Commit | Line | Data |
---|---|---|
d646960f SO |
1 | /* |
2 | * Copyright (C) 2011 Intel Corporation. All rights reserved. | |
3 | * | |
4 | * This program is free software; you can redistribute it and/or modify | |
5 | * it under the terms of the GNU General Public License as published by | |
6 | * the Free Software Foundation; either version 2 of the License, or | |
7 | * (at your option) any later version. | |
8 | * | |
9 | * This program is distributed in the hope that it will be useful, | |
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
12 | * GNU General Public License for more details. | |
13 | * | |
14 | * You should have received a copy of the GNU General Public License | |
15 | * along with this program; if not, write to the | |
16 | * Free Software Foundation, Inc., | |
17 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. | |
18 | */ | |
19 | ||
20 | #define pr_fmt(fmt) "llcp: %s: " fmt, __func__ | |
21 | ||
22 | #include <linux/init.h> | |
23 | #include <linux/kernel.h> | |
24 | #include <linux/list.h> | |
25 | #include <linux/nfc.h> | |
26 | ||
27 | #include "../nfc.h" | |
28 | #include "llcp.h" | |
29 | ||
30 | static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; | |
31 | ||
32 | static struct list_head llcp_devices; | |
33 | ||
a69f32af | 34 | void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk) |
d646960f | 35 | { |
a69f32af SO |
36 | write_lock(&l->lock); |
37 | sk_add_node(sk, &l->head); | |
38 | write_unlock(&l->lock); | |
39 | } | |
d646960f | 40 | |
a69f32af SO |
41 | void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk) |
42 | { | |
43 | write_lock(&l->lock); | |
44 | sk_del_node_init(sk); | |
45 | write_unlock(&l->lock); | |
46 | } | |
d646960f | 47 | |
a69f32af SO |
48 | static void nfc_llcp_socket_release(struct nfc_llcp_local *local) |
49 | { | |
50 | struct sock *sk; | |
51 | struct hlist_node *node, *tmp; | |
52 | struct nfc_llcp_sock *llcp_sock; | |
d646960f | 53 | |
a69f32af | 54 | write_lock(&local->sockets.lock); |
40c75f81 | 55 | |
a69f32af SO |
56 | sk_for_each_safe(sk, node, tmp, &local->sockets.head) { |
57 | llcp_sock = nfc_llcp_sock(sk); | |
d646960f | 58 | |
a69f32af | 59 | lock_sock(sk); |
d646960f | 60 | |
a69f32af SO |
61 | if (sk->sk_state == LLCP_CONNECTED) |
62 | nfc_put_device(llcp_sock->dev); | |
d646960f | 63 | |
a69f32af | 64 | if (sk->sk_state == LLCP_LISTEN) { |
d646960f SO |
65 | struct nfc_llcp_sock *lsk, *n; |
66 | struct sock *accept_sk; | |
67 | ||
a69f32af | 68 | list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue, |
427a2eb1 | 69 | accept_queue) { |
d646960f SO |
70 | accept_sk = &lsk->sk; |
71 | lock_sock(accept_sk); | |
72 | ||
73 | nfc_llcp_accept_unlink(accept_sk); | |
74 | ||
75 | accept_sk->sk_state = LLCP_CLOSED; | |
d646960f SO |
76 | |
77 | release_sock(accept_sk); | |
78 | ||
79 | sock_orphan(accept_sk); | |
80 | } | |
81 | } | |
82 | ||
a69f32af | 83 | sk->sk_state = LLCP_CLOSED; |
d646960f | 84 | |
a69f32af | 85 | release_sock(sk); |
d646960f | 86 | |
a69f32af | 87 | sock_orphan(sk); |
40c75f81 | 88 | |
a69f32af | 89 | sk_del_node_init(sk); |
d646960f SO |
90 | } |
91 | ||
a69f32af | 92 | write_unlock(&local->sockets.lock); |
d646960f SO |
93 | } |
94 | ||
c7aa1225 SO |
95 | struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) |
96 | { | |
97 | kref_get(&local->ref); | |
98 | ||
99 | return local; | |
100 | } | |
101 | ||
102 | static void local_release(struct kref *ref) | |
103 | { | |
104 | struct nfc_llcp_local *local; | |
105 | ||
106 | local = container_of(ref, struct nfc_llcp_local, ref); | |
107 | ||
108 | list_del(&local->list); | |
109 | nfc_llcp_socket_release(local); | |
110 | del_timer_sync(&local->link_timer); | |
111 | skb_queue_purge(&local->tx_queue); | |
112 | destroy_workqueue(local->tx_wq); | |
113 | destroy_workqueue(local->rx_wq); | |
114 | kfree_skb(local->rx_pending); | |
115 | kfree(local); | |
116 | } | |
117 | ||
118 | int nfc_llcp_local_put(struct nfc_llcp_local *local) | |
119 | { | |
a69f32af SO |
120 | WARN_ON(local == NULL); |
121 | ||
122 | if (local == NULL) | |
123 | return 0; | |
124 | ||
c7aa1225 SO |
125 | return kref_put(&local->ref, local_release); |
126 | } | |
127 | ||
b9a76f1d SO |
128 | static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local) |
129 | { | |
130 | mutex_lock(&local->sdp_lock); | |
131 | ||
132 | local->local_wks = 0; | |
133 | local->local_sdp = 0; | |
134 | local->local_sap = 0; | |
135 | ||
136 | mutex_unlock(&local->sdp_lock); | |
137 | } | |
138 | ||
d646960f SO |
139 | static void nfc_llcp_timeout_work(struct work_struct *work) |
140 | { | |
141 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
427a2eb1 | 142 | timeout_work); |
d646960f SO |
143 | |
144 | nfc_dep_link_down(local->dev); | |
145 | } | |
146 | ||
147 | static void nfc_llcp_symm_timer(unsigned long data) | |
148 | { | |
149 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; | |
150 | ||
151 | pr_err("SYMM timeout\n"); | |
152 | ||
153 | queue_work(local->timeout_wq, &local->timeout_work); | |
154 | } | |
155 | ||
156 | struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) | |
157 | { | |
158 | struct nfc_llcp_local *local, *n; | |
159 | ||
160 | list_for_each_entry_safe(local, n, &llcp_devices, list) | |
161 | if (local->dev == dev) | |
162 | return local; | |
163 | ||
164 | pr_debug("No device found\n"); | |
165 | ||
166 | return NULL; | |
167 | } | |
168 | ||
169 | static char *wks[] = { | |
170 | NULL, | |
171 | NULL, /* SDP */ | |
172 | "urn:nfc:sn:ip", | |
173 | "urn:nfc:sn:obex", | |
174 | "urn:nfc:sn:snep", | |
175 | }; | |
176 | ||
177 | static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len) | |
178 | { | |
179 | int sap, num_wks; | |
180 | ||
181 | pr_debug("%s\n", service_name); | |
182 | ||
183 | if (service_name == NULL) | |
184 | return -EINVAL; | |
185 | ||
186 | num_wks = ARRAY_SIZE(wks); | |
187 | ||
427a2eb1 | 188 | for (sap = 0; sap < num_wks; sap++) { |
d646960f SO |
189 | if (wks[sap] == NULL) |
190 | continue; | |
191 | ||
192 | if (strncmp(wks[sap], service_name, service_name_len) == 0) | |
193 | return sap; | |
194 | } | |
195 | ||
196 | return -EINVAL; | |
197 | } | |
198 | ||
199 | u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, | |
427a2eb1 | 200 | struct nfc_llcp_sock *sock) |
d646960f SO |
201 | { |
202 | mutex_lock(&local->sdp_lock); | |
203 | ||
204 | if (sock->service_name != NULL && sock->service_name_len > 0) { | |
205 | int ssap = nfc_llcp_wks_sap(sock->service_name, | |
427a2eb1 | 206 | sock->service_name_len); |
d646960f SO |
207 | |
208 | if (ssap > 0) { | |
209 | pr_debug("WKS %d\n", ssap); | |
210 | ||
211 | /* This is a WKS, let's check if it's free */ | |
212 | if (local->local_wks & BIT(ssap)) { | |
213 | mutex_unlock(&local->sdp_lock); | |
214 | ||
215 | return LLCP_SAP_MAX; | |
216 | } | |
217 | ||
1762c17c | 218 | set_bit(ssap, &local->local_wks); |
d646960f SO |
219 | mutex_unlock(&local->sdp_lock); |
220 | ||
221 | return ssap; | |
222 | } | |
223 | ||
224 | /* | |
225 | * This is not a well known service, | |
226 | * we should try to find a local SDP free spot | |
227 | */ | |
228 | ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); | |
229 | if (ssap == LLCP_SDP_NUM_SAP) { | |
230 | mutex_unlock(&local->sdp_lock); | |
231 | ||
232 | return LLCP_SAP_MAX; | |
233 | } | |
234 | ||
235 | pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); | |
236 | ||
1762c17c | 237 | set_bit(ssap, &local->local_sdp); |
d646960f SO |
238 | mutex_unlock(&local->sdp_lock); |
239 | ||
240 | return LLCP_WKS_NUM_SAP + ssap; | |
241 | ||
242 | } else if (sock->ssap != 0) { | |
243 | if (sock->ssap < LLCP_WKS_NUM_SAP) { | |
1762c17c SO |
244 | if (!test_bit(sock->ssap, &local->local_wks)) { |
245 | set_bit(sock->ssap, &local->local_wks); | |
d646960f SO |
246 | mutex_unlock(&local->sdp_lock); |
247 | ||
248 | return sock->ssap; | |
249 | } | |
250 | ||
251 | } else if (sock->ssap < LLCP_SDP_NUM_SAP) { | |
1762c17c SO |
252 | if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP, |
253 | &local->local_sdp)) { | |
254 | set_bit(sock->ssap - LLCP_WKS_NUM_SAP, | |
255 | &local->local_sdp); | |
d646960f SO |
256 | mutex_unlock(&local->sdp_lock); |
257 | ||
258 | return sock->ssap; | |
259 | } | |
260 | } | |
261 | } | |
262 | ||
263 | mutex_unlock(&local->sdp_lock); | |
264 | ||
265 | return LLCP_SAP_MAX; | |
266 | } | |
267 | ||
268 | u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) | |
269 | { | |
270 | u8 local_ssap; | |
271 | ||
272 | mutex_lock(&local->sdp_lock); | |
273 | ||
274 | local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); | |
275 | if (local_ssap == LLCP_LOCAL_NUM_SAP) { | |
276 | mutex_unlock(&local->sdp_lock); | |
277 | return LLCP_SAP_MAX; | |
278 | } | |
279 | ||
1762c17c | 280 | set_bit(local_ssap, &local->local_sap); |
d646960f SO |
281 | |
282 | mutex_unlock(&local->sdp_lock); | |
283 | ||
284 | return local_ssap + LLCP_LOCAL_SAP_OFFSET; | |
285 | } | |
286 | ||
287 | void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) | |
288 | { | |
289 | u8 local_ssap; | |
290 | unsigned long *sdp; | |
291 | ||
292 | if (ssap < LLCP_WKS_NUM_SAP) { | |
293 | local_ssap = ssap; | |
294 | sdp = &local->local_wks; | |
295 | } else if (ssap < LLCP_LOCAL_NUM_SAP) { | |
296 | local_ssap = ssap - LLCP_WKS_NUM_SAP; | |
297 | sdp = &local->local_sdp; | |
298 | } else if (ssap < LLCP_MAX_SAP) { | |
299 | local_ssap = ssap - LLCP_LOCAL_NUM_SAP; | |
300 | sdp = &local->local_sap; | |
301 | } else { | |
302 | return; | |
303 | } | |
304 | ||
305 | mutex_lock(&local->sdp_lock); | |
306 | ||
1762c17c | 307 | clear_bit(local_ssap, sdp); |
d646960f SO |
308 | |
309 | mutex_unlock(&local->sdp_lock); | |
310 | } | |
311 | ||
47807d3d | 312 | u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) |
d646960f SO |
313 | { |
314 | struct nfc_llcp_local *local; | |
315 | ||
316 | local = nfc_llcp_find_local(dev); | |
317 | if (local == NULL) { | |
318 | *general_bytes_len = 0; | |
319 | return NULL; | |
320 | } | |
321 | ||
322 | *general_bytes_len = local->gb_len; | |
323 | ||
324 | return local->gb; | |
325 | } | |
326 | ||
327 | static int nfc_llcp_build_gb(struct nfc_llcp_local *local) | |
328 | { | |
329 | u8 *gb_cur, *version_tlv, version, version_length; | |
330 | u8 *lto_tlv, lto, lto_length; | |
331 | u8 *wks_tlv, wks_length; | |
56d5876a SO |
332 | u8 *miux_tlv, miux_length; |
333 | __be16 miux; | |
d646960f SO |
334 | u8 gb_len = 0; |
335 | ||
336 | version = LLCP_VERSION_11; | |
337 | version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, | |
427a2eb1 | 338 | 1, &version_length); |
d646960f SO |
339 | gb_len += version_length; |
340 | ||
341 | /* 1500 ms */ | |
342 | lto = 150; | |
91b0ade1 | 343 | lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, <o, 1, <o_length); |
d646960f SO |
344 | gb_len += lto_length; |
345 | ||
346 | pr_debug("Local wks 0x%lx\n", local->local_wks); | |
347 | wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, | |
427a2eb1 | 348 | &wks_length); |
d646960f SO |
349 | gb_len += wks_length; |
350 | ||
56d5876a SO |
351 | miux = cpu_to_be16(LLCP_MAX_MIUX); |
352 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, | |
353 | &miux_length); | |
354 | gb_len += miux_length; | |
355 | ||
d646960f SO |
356 | gb_len += ARRAY_SIZE(llcp_magic); |
357 | ||
358 | if (gb_len > NFC_MAX_GT_LEN) { | |
359 | kfree(version_tlv); | |
360 | return -EINVAL; | |
361 | } | |
362 | ||
363 | gb_cur = local->gb; | |
364 | ||
365 | memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); | |
366 | gb_cur += ARRAY_SIZE(llcp_magic); | |
367 | ||
368 | memcpy(gb_cur, version_tlv, version_length); | |
369 | gb_cur += version_length; | |
370 | ||
371 | memcpy(gb_cur, lto_tlv, lto_length); | |
372 | gb_cur += lto_length; | |
373 | ||
374 | memcpy(gb_cur, wks_tlv, wks_length); | |
375 | gb_cur += wks_length; | |
376 | ||
56d5876a SO |
377 | memcpy(gb_cur, miux_tlv, miux_length); |
378 | gb_cur += miux_length; | |
379 | ||
d646960f SO |
380 | kfree(version_tlv); |
381 | kfree(lto_tlv); | |
382 | ||
383 | local->gb_len = gb_len; | |
384 | ||
385 | return 0; | |
386 | } | |
387 | ||
388 | int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) | |
389 | { | |
390 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); | |
391 | ||
392 | if (local == NULL) { | |
393 | pr_err("No LLCP device\n"); | |
394 | return -ENODEV; | |
395 | } | |
396 | ||
397 | memset(local->remote_gb, 0, NFC_MAX_GT_LEN); | |
398 | memcpy(local->remote_gb, gb, gb_len); | |
399 | local->remote_gb_len = gb_len; | |
400 | ||
427a2eb1 | 401 | if (local->remote_gb == NULL || local->remote_gb_len == 0) |
d646960f SO |
402 | return -ENODEV; |
403 | ||
404 | if (memcmp(local->remote_gb, llcp_magic, 3)) { | |
405 | pr_err("MAC does not support LLCP\n"); | |
406 | return -EINVAL; | |
407 | } | |
408 | ||
7a06e586 SO |
409 | return nfc_llcp_parse_gb_tlv(local, |
410 | &local->remote_gb[3], | |
411 | local->remote_gb_len - 3); | |
d646960f SO |
412 | } |
413 | ||
414 | static void nfc_llcp_tx_work(struct work_struct *work) | |
415 | { | |
416 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
427a2eb1 | 417 | tx_work); |
d646960f SO |
418 | struct sk_buff *skb; |
419 | ||
420 | skb = skb_dequeue(&local->tx_queue); | |
421 | if (skb != NULL) { | |
422 | pr_debug("Sending pending skb\n"); | |
4be646ec SO |
423 | print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET, |
424 | 16, 1, skb->data, skb->len, true); | |
425 | ||
d646960f | 426 | nfc_data_exchange(local->dev, local->target_idx, |
427a2eb1 | 427 | skb, nfc_llcp_recv, local); |
d646960f SO |
428 | } else { |
429 | nfc_llcp_send_symm(local->dev); | |
430 | } | |
431 | ||
432 | mod_timer(&local->link_timer, | |
427a2eb1 | 433 | jiffies + msecs_to_jiffies(local->remote_lto)); |
d646960f SO |
434 | } |
435 | ||
436 | static u8 nfc_llcp_dsap(struct sk_buff *pdu) | |
437 | { | |
438 | return (pdu->data[0] & 0xfc) >> 2; | |
439 | } | |
440 | ||
441 | static u8 nfc_llcp_ptype(struct sk_buff *pdu) | |
442 | { | |
443 | return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); | |
444 | } | |
445 | ||
446 | static u8 nfc_llcp_ssap(struct sk_buff *pdu) | |
447 | { | |
448 | return pdu->data[1] & 0x3f; | |
449 | } | |
450 | ||
451 | static u8 nfc_llcp_ns(struct sk_buff *pdu) | |
452 | { | |
453 | return pdu->data[2] >> 4; | |
454 | } | |
455 | ||
456 | static u8 nfc_llcp_nr(struct sk_buff *pdu) | |
457 | { | |
458 | return pdu->data[2] & 0xf; | |
459 | } | |
460 | ||
461 | static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) | |
462 | { | |
279cf174 | 463 | pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); |
d646960f SO |
464 | sock->send_n = (sock->send_n + 1) % 16; |
465 | sock->recv_ack_n = (sock->recv_n - 1) % 16; | |
466 | } | |
467 | ||
a69f32af SO |
468 | static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local, |
469 | u8 ssap) | |
470 | { | |
471 | struct sock *sk; | |
472 | struct nfc_llcp_sock *llcp_sock; | |
473 | struct hlist_node *node; | |
474 | ||
475 | read_lock(&local->connecting_sockets.lock); | |
476 | ||
477 | sk_for_each(sk, node, &local->connecting_sockets.head) { | |
478 | llcp_sock = nfc_llcp_sock(sk); | |
479 | ||
480 | if (llcp_sock->ssap == ssap) | |
481 | goto out; | |
482 | } | |
483 | ||
484 | llcp_sock = NULL; | |
485 | ||
486 | out: | |
487 | read_unlock(&local->connecting_sockets.lock); | |
488 | ||
489 | sock_hold(&llcp_sock->sk); | |
490 | ||
491 | return llcp_sock; | |
492 | } | |
493 | ||
d646960f | 494 | static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, |
427a2eb1 | 495 | u8 ssap, u8 dsap) |
d646960f | 496 | { |
a69f32af SO |
497 | struct sock *sk; |
498 | struct hlist_node *node; | |
499 | struct nfc_llcp_sock *llcp_sock; | |
d646960f | 500 | |
ff353d86 SO |
501 | pr_debug("ssap dsap %d %d\n", ssap, dsap); |
502 | ||
d646960f SO |
503 | if (ssap == 0 && dsap == 0) |
504 | return NULL; | |
505 | ||
a69f32af SO |
506 | read_lock(&local->sockets.lock); |
507 | ||
508 | llcp_sock = NULL; | |
d646960f | 509 | |
a69f32af SO |
510 | sk_for_each(sk, node, &local->sockets.head) { |
511 | llcp_sock = nfc_llcp_sock(sk); | |
d646960f | 512 | |
a69f32af SO |
513 | if (llcp_sock->ssap == ssap && |
514 | llcp_sock->dsap == dsap) | |
515 | break; | |
d646960f SO |
516 | } |
517 | ||
a69f32af SO |
518 | read_unlock(&local->sockets.lock); |
519 | ||
520 | if (llcp_sock == NULL) | |
521 | return NULL; | |
522 | ||
523 | sock_hold(&llcp_sock->sk); | |
524 | ||
525 | return llcp_sock; | |
526 | } | |
527 | ||
528 | static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local, | |
529 | u8 *sn, size_t sn_len) | |
530 | { | |
531 | struct sock *sk; | |
532 | struct hlist_node *node; | |
533 | struct nfc_llcp_sock *llcp_sock; | |
534 | ||
535 | pr_debug("sn %zd\n", sn_len); | |
536 | ||
537 | if (sn == NULL || sn_len == 0) | |
538 | return NULL; | |
539 | ||
540 | read_lock(&local->sockets.lock); | |
541 | ||
542 | llcp_sock = NULL; | |
543 | ||
544 | sk_for_each(sk, node, &local->sockets.head) { | |
545 | llcp_sock = nfc_llcp_sock(sk); | |
546 | ||
547 | if (llcp_sock->sk.sk_state != LLCP_LISTEN) | |
548 | continue; | |
549 | ||
550 | if (llcp_sock->service_name == NULL || | |
551 | llcp_sock->service_name_len == 0) | |
552 | continue; | |
553 | ||
554 | if (llcp_sock->service_name_len != sn_len) | |
555 | continue; | |
556 | ||
557 | if (memcmp(sn, llcp_sock->service_name, sn_len) == 0) | |
558 | break; | |
d646960f SO |
559 | } |
560 | ||
a69f32af | 561 | read_unlock(&local->sockets.lock); |
d646960f | 562 | |
a69f32af SO |
563 | if (llcp_sock == NULL) |
564 | return NULL; | |
d646960f | 565 | |
a69f32af SO |
566 | sock_hold(&llcp_sock->sk); |
567 | ||
568 | return llcp_sock; | |
d646960f SO |
569 | } |
570 | ||
571 | static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) | |
572 | { | |
573 | sock_put(&sock->sk); | |
574 | } | |
575 | ||
576 | static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) | |
577 | { | |
578 | u8 *tlv = &skb->data[2], type, length; | |
579 | size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; | |
580 | ||
581 | while (offset < tlv_array_len) { | |
582 | type = tlv[0]; | |
583 | length = tlv[1]; | |
584 | ||
585 | pr_debug("type 0x%x length %d\n", type, length); | |
586 | ||
587 | if (type == LLCP_TLV_SN) { | |
588 | *sn_len = length; | |
589 | return &tlv[2]; | |
590 | } | |
591 | ||
592 | offset += length + 2; | |
593 | tlv += length + 2; | |
594 | } | |
595 | ||
596 | return NULL; | |
597 | } | |
598 | ||
599 | static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, | |
427a2eb1 | 600 | struct sk_buff *skb) |
d646960f SO |
601 | { |
602 | struct sock *new_sk, *parent; | |
603 | struct nfc_llcp_sock *sock, *new_sock; | |
a69f32af | 604 | u8 dsap, ssap, reason; |
d646960f SO |
605 | |
606 | dsap = nfc_llcp_dsap(skb); | |
607 | ssap = nfc_llcp_ssap(skb); | |
608 | ||
609 | pr_debug("%d %d\n", dsap, ssap); | |
610 | ||
d646960f | 611 | if (dsap != LLCP_SAP_SDP) { |
a69f32af SO |
612 | sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); |
613 | if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) { | |
d646960f SO |
614 | reason = LLCP_DM_NOBOUND; |
615 | goto fail; | |
616 | } | |
d646960f SO |
617 | } else { |
618 | u8 *sn; | |
619 | size_t sn_len; | |
620 | ||
621 | sn = nfc_llcp_connect_sn(skb, &sn_len); | |
622 | if (sn == NULL) { | |
623 | reason = LLCP_DM_NOBOUND; | |
624 | goto fail; | |
625 | } | |
626 | ||
627 | pr_debug("Service name length %zu\n", sn_len); | |
628 | ||
a69f32af SO |
629 | sock = nfc_llcp_sock_get_sn(local, sn, sn_len); |
630 | if (sock == NULL) { | |
631 | reason = LLCP_DM_NOBOUND; | |
632 | goto fail; | |
d646960f | 633 | } |
d646960f SO |
634 | } |
635 | ||
a69f32af | 636 | lock_sock(&sock->sk); |
d646960f | 637 | |
d646960f SO |
638 | parent = &sock->sk; |
639 | ||
640 | if (sk_acceptq_is_full(parent)) { | |
641 | reason = LLCP_DM_REJ; | |
642 | release_sock(&sock->sk); | |
643 | sock_put(&sock->sk); | |
644 | goto fail; | |
645 | } | |
646 | ||
427a2eb1 | 647 | new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC); |
d646960f SO |
648 | if (new_sk == NULL) { |
649 | reason = LLCP_DM_REJ; | |
650 | release_sock(&sock->sk); | |
651 | sock_put(&sock->sk); | |
652 | goto fail; | |
653 | } | |
654 | ||
655 | new_sock = nfc_llcp_sock(new_sk); | |
656 | new_sock->dev = local->dev; | |
c7aa1225 | 657 | new_sock->local = nfc_llcp_local_get(local); |
93d7e490 | 658 | new_sock->miu = local->remote_miu; |
d646960f | 659 | new_sock->nfc_protocol = sock->nfc_protocol; |
a69f32af | 660 | new_sock->ssap = sock->ssap; |
d646960f SO |
661 | new_sock->dsap = ssap; |
662 | new_sock->parent = parent; | |
663 | ||
7a06e586 SO |
664 | nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE], |
665 | skb->len - LLCP_HEADER_SIZE); | |
666 | ||
d646960f SO |
667 | pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); |
668 | ||
a69f32af | 669 | nfc_llcp_sock_link(&local->sockets, new_sk); |
d646960f SO |
670 | |
671 | nfc_llcp_accept_enqueue(&sock->sk, new_sk); | |
672 | ||
673 | nfc_get_device(local->dev->idx); | |
674 | ||
675 | new_sk->sk_state = LLCP_CONNECTED; | |
676 | ||
677 | /* Wake the listening processes */ | |
678 | parent->sk_data_ready(parent, 0); | |
679 | ||
680 | /* Send CC */ | |
681 | nfc_llcp_send_cc(new_sock); | |
682 | ||
683 | release_sock(&sock->sk); | |
684 | sock_put(&sock->sk); | |
685 | ||
686 | return; | |
687 | ||
688 | fail: | |
689 | /* Send DM */ | |
690 | nfc_llcp_send_dm(local, dsap, ssap, reason); | |
691 | ||
692 | return; | |
693 | ||
694 | } | |
695 | ||
d094afa1 | 696 | int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) |
4722d2b7 | 697 | { |
d094afa1 | 698 | int nr_frames = 0; |
4722d2b7 SO |
699 | struct nfc_llcp_local *local = sock->local; |
700 | ||
701 | pr_debug("Remote ready %d tx queue len %d remote rw %d", | |
427a2eb1 | 702 | sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), |
7a06e586 | 703 | sock->rw); |
4722d2b7 SO |
704 | |
705 | /* Try to queue some I frames for transmission */ | |
706 | while (sock->remote_ready && | |
7a06e586 | 707 | skb_queue_len(&sock->tx_pending_queue) < sock->rw) { |
4722d2b7 SO |
708 | struct sk_buff *pdu, *pending_pdu; |
709 | ||
710 | pdu = skb_dequeue(&sock->tx_queue); | |
711 | if (pdu == NULL) | |
712 | break; | |
713 | ||
714 | /* Update N(S)/N(R) */ | |
715 | nfc_llcp_set_nrns(sock, pdu); | |
716 | ||
717 | pending_pdu = skb_clone(pdu, GFP_KERNEL); | |
718 | ||
719 | skb_queue_tail(&local->tx_queue, pdu); | |
720 | skb_queue_tail(&sock->tx_pending_queue, pending_pdu); | |
d094afa1 | 721 | nr_frames++; |
4722d2b7 | 722 | } |
d094afa1 SO |
723 | |
724 | return nr_frames; | |
4722d2b7 SO |
725 | } |
726 | ||
d646960f | 727 | static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, |
427a2eb1 | 728 | struct sk_buff *skb) |
d646960f SO |
729 | { |
730 | struct nfc_llcp_sock *llcp_sock; | |
731 | struct sock *sk; | |
732 | u8 dsap, ssap, ptype, ns, nr; | |
733 | ||
734 | ptype = nfc_llcp_ptype(skb); | |
735 | dsap = nfc_llcp_dsap(skb); | |
736 | ssap = nfc_llcp_ssap(skb); | |
737 | ns = nfc_llcp_ns(skb); | |
738 | nr = nfc_llcp_nr(skb); | |
739 | ||
740 | pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); | |
741 | ||
742 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
743 | if (llcp_sock == NULL) { | |
744 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
745 | return; | |
746 | } | |
747 | ||
748 | sk = &llcp_sock->sk; | |
749 | lock_sock(sk); | |
750 | if (sk->sk_state == LLCP_CLOSED) { | |
751 | release_sock(sk); | |
752 | nfc_llcp_sock_put(llcp_sock); | |
753 | } | |
754 | ||
d646960f SO |
755 | /* Pass the payload upstream */ |
756 | if (ptype == LLCP_PDU_I) { | |
757 | pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); | |
758 | ||
53aef920 SO |
759 | if (ns == llcp_sock->recv_n) |
760 | llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; | |
761 | else | |
762 | pr_err("Received out of sequence I PDU\n"); | |
763 | ||
d646960f SO |
764 | skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); |
765 | if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { | |
766 | pr_err("receive queue is full\n"); | |
767 | skb_queue_head(&llcp_sock->tx_backlog_queue, skb); | |
768 | } | |
769 | } | |
770 | ||
771 | /* Remove skbs from the pending queue */ | |
772 | if (llcp_sock->send_ack_n != nr) { | |
773 | struct sk_buff *s, *tmp; | |
774 | ||
775 | llcp_sock->send_ack_n = nr; | |
776 | ||
777 | skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) | |
778 | if (nfc_llcp_ns(s) <= nr) { | |
779 | skb_unlink(s, &llcp_sock->tx_pending_queue); | |
780 | kfree_skb(s); | |
781 | } | |
782 | } | |
783 | ||
53aef920 SO |
784 | if (ptype == LLCP_PDU_RR) |
785 | llcp_sock->remote_ready = true; | |
427a2eb1 | 786 | else if (ptype == LLCP_PDU_RNR) |
53aef920 SO |
787 | llcp_sock->remote_ready = false; |
788 | ||
d094afa1 SO |
789 | if (nfc_llcp_queue_i_frames(llcp_sock) == 0) |
790 | nfc_llcp_send_rr(llcp_sock); | |
d646960f SO |
791 | |
792 | release_sock(sk); | |
793 | nfc_llcp_sock_put(llcp_sock); | |
794 | } | |
795 | ||
796 | static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, | |
427a2eb1 | 797 | struct sk_buff *skb) |
d646960f SO |
798 | { |
799 | struct nfc_llcp_sock *llcp_sock; | |
800 | struct sock *sk; | |
801 | u8 dsap, ssap; | |
802 | ||
803 | dsap = nfc_llcp_dsap(skb); | |
804 | ssap = nfc_llcp_ssap(skb); | |
805 | ||
806 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
807 | if (llcp_sock == NULL) { | |
808 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
809 | return; | |
810 | } | |
811 | ||
812 | sk = &llcp_sock->sk; | |
813 | lock_sock(sk); | |
814 | if (sk->sk_state == LLCP_CLOSED) { | |
815 | release_sock(sk); | |
816 | nfc_llcp_sock_put(llcp_sock); | |
817 | } | |
818 | ||
d646960f SO |
819 | if (sk->sk_state == LLCP_CONNECTED) { |
820 | nfc_put_device(local->dev); | |
821 | sk->sk_state = LLCP_CLOSED; | |
822 | sk->sk_state_change(sk); | |
823 | } | |
824 | ||
825 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); | |
826 | ||
827 | release_sock(sk); | |
828 | nfc_llcp_sock_put(llcp_sock); | |
829 | } | |
830 | ||
427a2eb1 | 831 | static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) |
d646960f SO |
832 | { |
833 | struct nfc_llcp_sock *llcp_sock; | |
ff353d86 | 834 | struct sock *sk; |
d646960f SO |
835 | u8 dsap, ssap; |
836 | ||
d646960f SO |
837 | dsap = nfc_llcp_dsap(skb); |
838 | ssap = nfc_llcp_ssap(skb); | |
839 | ||
a69f32af | 840 | llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); |
d646960f SO |
841 | if (llcp_sock == NULL) { |
842 | pr_err("Invalid CC\n"); | |
843 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
844 | ||
845 | return; | |
846 | } | |
847 | ||
ff353d86 | 848 | sk = &llcp_sock->sk; |
d646960f | 849 | |
a69f32af SO |
850 | /* Unlink from connecting and link to the client array */ |
851 | nfc_llcp_sock_unlink(&local->connecting_sockets, sk); | |
852 | nfc_llcp_sock_link(&local->sockets, sk); | |
853 | llcp_sock->dsap = ssap; | |
854 | ||
7a06e586 SO |
855 | nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], |
856 | skb->len - LLCP_HEADER_SIZE); | |
d646960f | 857 | |
ff353d86 SO |
858 | sk->sk_state = LLCP_CONNECTED; |
859 | sk->sk_state_change(sk); | |
860 | ||
d646960f SO |
861 | nfc_llcp_sock_put(llcp_sock); |
862 | } | |
863 | ||
864 | static void nfc_llcp_rx_work(struct work_struct *work) | |
865 | { | |
866 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
427a2eb1 | 867 | rx_work); |
d646960f SO |
868 | u8 dsap, ssap, ptype; |
869 | struct sk_buff *skb; | |
870 | ||
871 | skb = local->rx_pending; | |
872 | if (skb == NULL) { | |
873 | pr_debug("No pending SKB\n"); | |
874 | return; | |
875 | } | |
876 | ||
877 | ptype = nfc_llcp_ptype(skb); | |
878 | dsap = nfc_llcp_dsap(skb); | |
879 | ssap = nfc_llcp_ssap(skb); | |
880 | ||
881 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); | |
882 | ||
4be646ec SO |
883 | if (ptype != LLCP_PDU_SYMM) |
884 | print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, | |
885 | 16, 1, skb->data, skb->len, true); | |
886 | ||
d646960f SO |
887 | switch (ptype) { |
888 | case LLCP_PDU_SYMM: | |
889 | pr_debug("SYMM\n"); | |
890 | break; | |
891 | ||
892 | case LLCP_PDU_CONNECT: | |
893 | pr_debug("CONNECT\n"); | |
894 | nfc_llcp_recv_connect(local, skb); | |
895 | break; | |
896 | ||
897 | case LLCP_PDU_DISC: | |
898 | pr_debug("DISC\n"); | |
899 | nfc_llcp_recv_disc(local, skb); | |
900 | break; | |
901 | ||
902 | case LLCP_PDU_CC: | |
903 | pr_debug("CC\n"); | |
904 | nfc_llcp_recv_cc(local, skb); | |
905 | break; | |
906 | ||
907 | case LLCP_PDU_I: | |
908 | case LLCP_PDU_RR: | |
53aef920 | 909 | case LLCP_PDU_RNR: |
d646960f SO |
910 | pr_debug("I frame\n"); |
911 | nfc_llcp_recv_hdlc(local, skb); | |
912 | break; | |
913 | ||
914 | } | |
915 | ||
916 | queue_work(local->tx_wq, &local->tx_work); | |
917 | kfree_skb(local->rx_pending); | |
918 | local->rx_pending = NULL; | |
919 | ||
920 | return; | |
921 | } | |
922 | ||
923 | void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) | |
924 | { | |
925 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; | |
926 | ||
927 | pr_debug("Received an LLCP PDU\n"); | |
928 | if (err < 0) { | |
427a2eb1 | 929 | pr_err("err %d\n", err); |
d646960f SO |
930 | return; |
931 | } | |
932 | ||
933 | local->rx_pending = skb_get(skb); | |
934 | del_timer(&local->link_timer); | |
935 | queue_work(local->rx_wq, &local->rx_work); | |
936 | ||
937 | return; | |
938 | } | |
939 | ||
940 | void nfc_llcp_mac_is_down(struct nfc_dev *dev) | |
941 | { | |
942 | struct nfc_llcp_local *local; | |
943 | ||
944 | local = nfc_llcp_find_local(dev); | |
945 | if (local == NULL) | |
946 | return; | |
947 | ||
b9a76f1d SO |
948 | nfc_llcp_clear_sdp(local); |
949 | ||
d646960f SO |
950 | /* Close and purge all existing sockets */ |
951 | nfc_llcp_socket_release(local); | |
952 | } | |
953 | ||
954 | void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, | |
955 | u8 comm_mode, u8 rf_mode) | |
956 | { | |
957 | struct nfc_llcp_local *local; | |
958 | ||
959 | pr_debug("rf mode %d\n", rf_mode); | |
960 | ||
961 | local = nfc_llcp_find_local(dev); | |
962 | if (local == NULL) | |
963 | return; | |
964 | ||
965 | local->target_idx = target_idx; | |
966 | local->comm_mode = comm_mode; | |
967 | local->rf_mode = rf_mode; | |
968 | ||
969 | if (rf_mode == NFC_RF_INITIATOR) { | |
970 | pr_debug("Queueing Tx work\n"); | |
971 | ||
972 | queue_work(local->tx_wq, &local->tx_work); | |
973 | } else { | |
974 | mod_timer(&local->link_timer, | |
427a2eb1 | 975 | jiffies + msecs_to_jiffies(local->remote_lto)); |
d646960f SO |
976 | } |
977 | } | |
978 | ||
979 | int nfc_llcp_register_device(struct nfc_dev *ndev) | |
980 | { | |
981 | struct device *dev = &ndev->dev; | |
982 | struct nfc_llcp_local *local; | |
983 | char name[32]; | |
984 | int err; | |
985 | ||
986 | local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); | |
987 | if (local == NULL) | |
988 | return -ENOMEM; | |
989 | ||
990 | local->dev = ndev; | |
991 | INIT_LIST_HEAD(&local->list); | |
c7aa1225 | 992 | kref_init(&local->ref); |
d646960f | 993 | mutex_init(&local->sdp_lock); |
d646960f SO |
994 | init_timer(&local->link_timer); |
995 | local->link_timer.data = (unsigned long) local; | |
996 | local->link_timer.function = nfc_llcp_symm_timer; | |
997 | ||
998 | skb_queue_head_init(&local->tx_queue); | |
999 | INIT_WORK(&local->tx_work, nfc_llcp_tx_work); | |
1000 | snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); | |
427a2eb1 SO |
1001 | local->tx_wq = |
1002 | alloc_workqueue(name, | |
1003 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, | |
1004 | 1); | |
d646960f SO |
1005 | if (local->tx_wq == NULL) { |
1006 | err = -ENOMEM; | |
1007 | goto err_local; | |
1008 | } | |
1009 | ||
1010 | local->rx_pending = NULL; | |
1011 | INIT_WORK(&local->rx_work, nfc_llcp_rx_work); | |
1012 | snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); | |
427a2eb1 SO |
1013 | local->rx_wq = |
1014 | alloc_workqueue(name, | |
1015 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, | |
1016 | 1); | |
d646960f SO |
1017 | if (local->rx_wq == NULL) { |
1018 | err = -ENOMEM; | |
1019 | goto err_tx_wq; | |
1020 | } | |
1021 | ||
1022 | INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); | |
1023 | snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); | |
427a2eb1 SO |
1024 | local->timeout_wq = |
1025 | alloc_workqueue(name, | |
1026 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, | |
1027 | 1); | |
d646960f SO |
1028 | if (local->timeout_wq == NULL) { |
1029 | err = -ENOMEM; | |
1030 | goto err_rx_wq; | |
1031 | } | |
1032 | ||
a69f32af SO |
1033 | local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock); |
1034 | local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock); | |
1035 | ||
d646960f SO |
1036 | nfc_llcp_build_gb(local); |
1037 | ||
1038 | local->remote_miu = LLCP_DEFAULT_MIU; | |
1039 | local->remote_lto = LLCP_DEFAULT_LTO; | |
d646960f SO |
1040 | |
1041 | list_add(&llcp_devices, &local->list); | |
1042 | ||
1043 | return 0; | |
1044 | ||
1045 | err_rx_wq: | |
1046 | destroy_workqueue(local->rx_wq); | |
1047 | ||
1048 | err_tx_wq: | |
1049 | destroy_workqueue(local->tx_wq); | |
1050 | ||
1051 | err_local: | |
1052 | kfree(local); | |
1053 | ||
1054 | return 0; | |
1055 | } | |
1056 | ||
1057 | void nfc_llcp_unregister_device(struct nfc_dev *dev) | |
1058 | { | |
1059 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); | |
1060 | ||
1061 | if (local == NULL) { | |
1062 | pr_debug("No such device\n"); | |
1063 | return; | |
1064 | } | |
1065 | ||
c7aa1225 | 1066 | nfc_llcp_local_put(local); |
d646960f SO |
1067 | } |
1068 | ||
1069 | int __init nfc_llcp_init(void) | |
1070 | { | |
1071 | INIT_LIST_HEAD(&llcp_devices); | |
1072 | ||
1073 | return nfc_llcp_sock_init(); | |
1074 | } | |
1075 | ||
1076 | void nfc_llcp_exit(void) | |
1077 | { | |
1078 | nfc_llcp_sock_exit(); | |
1079 | } |