aboutsummaryrefslogtreecommitdiffstats
path: root/net/nfc/llcp/llcp.c
diff options
context:
space:
mode:
Diffstat (limited to 'net/nfc/llcp/llcp.c')
-rw-r--r--net/nfc/llcp/llcp.c201
1 files changed, 122 insertions, 79 deletions
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 1d32680807d6..17a578f641f1 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -37,7 +37,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
37 struct sock *sk, *parent_sk; 37 struct sock *sk, *parent_sk;
38 int i; 38 int i;
39 39
40
41 mutex_lock(&local->socket_lock); 40 mutex_lock(&local->socket_lock);
42 41
43 for (i = 0; i < LLCP_MAX_SAP; i++) { 42 for (i = 0; i < LLCP_MAX_SAP; i++) {
@@ -47,7 +46,7 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
47 46
48 /* Release all child sockets */ 47 /* Release all child sockets */
49 list_for_each_entry_safe(s, n, &parent->list, list) { 48 list_for_each_entry_safe(s, n, &parent->list, list) {
50 list_del(&s->list); 49 list_del_init(&s->list);
51 sk = &s->sk; 50 sk = &s->sk;
52 51
53 lock_sock(sk); 52 lock_sock(sk);
@@ -56,9 +55,12 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
56 nfc_put_device(s->dev); 55 nfc_put_device(s->dev);
57 56
58 sk->sk_state = LLCP_CLOSED; 57 sk->sk_state = LLCP_CLOSED;
59 sock_set_flag(sk, SOCK_DEAD);
60 58
61 release_sock(sk); 59 release_sock(sk);
60
61 sock_orphan(sk);
62
63 s->local = NULL;
62 } 64 }
63 65
64 parent_sk = &parent->sk; 66 parent_sk = &parent->sk;
@@ -70,18 +72,19 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
70 struct sock *accept_sk; 72 struct sock *accept_sk;
71 73
72 list_for_each_entry_safe(lsk, n, &parent->accept_queue, 74 list_for_each_entry_safe(lsk, n, &parent->accept_queue,
73 accept_queue) { 75 accept_queue) {
74 accept_sk = &lsk->sk; 76 accept_sk = &lsk->sk;
75 lock_sock(accept_sk); 77 lock_sock(accept_sk);
76 78
77 nfc_llcp_accept_unlink(accept_sk); 79 nfc_llcp_accept_unlink(accept_sk);
78 80
79 accept_sk->sk_state = LLCP_CLOSED; 81 accept_sk->sk_state = LLCP_CLOSED;
80 sock_set_flag(accept_sk, SOCK_DEAD);
81 82
82 release_sock(accept_sk); 83 release_sock(accept_sk);
83 84
84 sock_orphan(accept_sk); 85 sock_orphan(accept_sk);
86
87 lsk->local = NULL;
85 } 88 }
86 } 89 }
87 90
@@ -89,18 +92,32 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
89 nfc_put_device(parent->dev); 92 nfc_put_device(parent->dev);
90 93
91 parent_sk->sk_state = LLCP_CLOSED; 94 parent_sk->sk_state = LLCP_CLOSED;
92 sock_set_flag(parent_sk, SOCK_DEAD);
93 95
94 release_sock(parent_sk); 96 release_sock(parent_sk);
97
98 sock_orphan(parent_sk);
99
100 parent->local = NULL;
95 } 101 }
96 102
97 mutex_unlock(&local->socket_lock); 103 mutex_unlock(&local->socket_lock);
98} 104}
99 105
106static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
107{
108 mutex_lock(&local->sdp_lock);
109
110 local->local_wks = 0;
111 local->local_sdp = 0;
112 local->local_sap = 0;
113
114 mutex_unlock(&local->sdp_lock);
115}
116
100static void nfc_llcp_timeout_work(struct work_struct *work) 117static void nfc_llcp_timeout_work(struct work_struct *work)
101{ 118{
102 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 119 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
103 timeout_work); 120 timeout_work);
104 121
105 nfc_dep_link_down(local->dev); 122 nfc_dep_link_down(local->dev);
106} 123}
@@ -146,7 +163,7 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
146 163
147 num_wks = ARRAY_SIZE(wks); 164 num_wks = ARRAY_SIZE(wks);
148 165
149 for (sap = 0 ; sap < num_wks; sap++) { 166 for (sap = 0; sap < num_wks; sap++) {
150 if (wks[sap] == NULL) 167 if (wks[sap] == NULL)
151 continue; 168 continue;
152 169
@@ -158,13 +175,13 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
158} 175}
159 176
160u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, 177u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
161 struct nfc_llcp_sock *sock) 178 struct nfc_llcp_sock *sock)
162{ 179{
163 mutex_lock(&local->sdp_lock); 180 mutex_lock(&local->sdp_lock);
164 181
165 if (sock->service_name != NULL && sock->service_name_len > 0) { 182 if (sock->service_name != NULL && sock->service_name_len > 0) {
166 int ssap = nfc_llcp_wks_sap(sock->service_name, 183 int ssap = nfc_llcp_wks_sap(sock->service_name,
167 sock->service_name_len); 184 sock->service_name_len);
168 185
169 if (ssap > 0) { 186 if (ssap > 0) {
170 pr_debug("WKS %d\n", ssap); 187 pr_debug("WKS %d\n", ssap);
@@ -176,7 +193,7 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
176 return LLCP_SAP_MAX; 193 return LLCP_SAP_MAX;
177 } 194 }
178 195
179 set_bit(BIT(ssap), &local->local_wks); 196 set_bit(ssap, &local->local_wks);
180 mutex_unlock(&local->sdp_lock); 197 mutex_unlock(&local->sdp_lock);
181 198
182 return ssap; 199 return ssap;
@@ -195,25 +212,25 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
195 212
196 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); 213 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
197 214
198 set_bit(BIT(ssap), &local->local_sdp); 215 set_bit(ssap, &local->local_sdp);
199 mutex_unlock(&local->sdp_lock); 216 mutex_unlock(&local->sdp_lock);
200 217
201 return LLCP_WKS_NUM_SAP + ssap; 218 return LLCP_WKS_NUM_SAP + ssap;
202 219
203 } else if (sock->ssap != 0) { 220 } else if (sock->ssap != 0) {
204 if (sock->ssap < LLCP_WKS_NUM_SAP) { 221 if (sock->ssap < LLCP_WKS_NUM_SAP) {
205 if (!(local->local_wks & BIT(sock->ssap))) { 222 if (!test_bit(sock->ssap, &local->local_wks)) {
206 set_bit(BIT(sock->ssap), &local->local_wks); 223 set_bit(sock->ssap, &local->local_wks);
207 mutex_unlock(&local->sdp_lock); 224 mutex_unlock(&local->sdp_lock);
208 225
209 return sock->ssap; 226 return sock->ssap;
210 } 227 }
211 228
212 } else if (sock->ssap < LLCP_SDP_NUM_SAP) { 229 } else if (sock->ssap < LLCP_SDP_NUM_SAP) {
213 if (!(local->local_sdp & 230 if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP,
214 BIT(sock->ssap - LLCP_WKS_NUM_SAP))) { 231 &local->local_sdp)) {
215 set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP), 232 set_bit(sock->ssap - LLCP_WKS_NUM_SAP,
216 &local->local_sdp); 233 &local->local_sdp);
217 mutex_unlock(&local->sdp_lock); 234 mutex_unlock(&local->sdp_lock);
218 235
219 return sock->ssap; 236 return sock->ssap;
@@ -238,7 +255,7 @@ u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
238 return LLCP_SAP_MAX; 255 return LLCP_SAP_MAX;
239 } 256 }
240 257
241 set_bit(BIT(local_ssap), &local->local_sap); 258 set_bit(local_ssap, &local->local_sap);
242 259
243 mutex_unlock(&local->sdp_lock); 260 mutex_unlock(&local->sdp_lock);
244 261
@@ -265,12 +282,12 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
265 282
266 mutex_lock(&local->sdp_lock); 283 mutex_lock(&local->sdp_lock);
267 284
268 clear_bit(1 << local_ssap, sdp); 285 clear_bit(local_ssap, sdp);
269 286
270 mutex_unlock(&local->sdp_lock); 287 mutex_unlock(&local->sdp_lock);
271} 288}
272 289
273u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len) 290u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
274{ 291{
275 struct nfc_llcp_local *local; 292 struct nfc_llcp_local *local;
276 293
@@ -294,7 +311,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
294 311
295 version = LLCP_VERSION_11; 312 version = LLCP_VERSION_11;
296 version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, 313 version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
297 1, &version_length); 314 1, &version_length);
298 gb_len += version_length; 315 gb_len += version_length;
299 316
300 /* 1500 ms */ 317 /* 1500 ms */
@@ -304,7 +321,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
304 321
305 pr_debug("Local wks 0x%lx\n", local->local_wks); 322 pr_debug("Local wks 0x%lx\n", local->local_wks);
306 wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, 323 wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
307 &wks_length); 324 &wks_length);
308 gb_len += wks_length; 325 gb_len += wks_length;
309 326
310 gb_len += ARRAY_SIZE(llcp_magic); 327 gb_len += ARRAY_SIZE(llcp_magic);
@@ -349,8 +366,7 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
349 memcpy(local->remote_gb, gb, gb_len); 366 memcpy(local->remote_gb, gb, gb_len);
350 local->remote_gb_len = gb_len; 367 local->remote_gb_len = gb_len;
351 368
352 if (local->remote_gb == NULL || 369 if (local->remote_gb == NULL || local->remote_gb_len == 0)
353 local->remote_gb_len == 0)
354 return -ENODEV; 370 return -ENODEV;
355 371
356 if (memcmp(local->remote_gb, llcp_magic, 3)) { 372 if (memcmp(local->remote_gb, llcp_magic, 3)) {
@@ -359,26 +375,27 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
359 } 375 }
360 376
361 return nfc_llcp_parse_tlv(local, 377 return nfc_llcp_parse_tlv(local,
362 &local->remote_gb[3], local->remote_gb_len - 3); 378 &local->remote_gb[3],
379 local->remote_gb_len - 3);
363} 380}
364 381
365static void nfc_llcp_tx_work(struct work_struct *work) 382static void nfc_llcp_tx_work(struct work_struct *work)
366{ 383{
367 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 384 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
368 tx_work); 385 tx_work);
369 struct sk_buff *skb; 386 struct sk_buff *skb;
370 387
371 skb = skb_dequeue(&local->tx_queue); 388 skb = skb_dequeue(&local->tx_queue);
372 if (skb != NULL) { 389 if (skb != NULL) {
373 pr_debug("Sending pending skb\n"); 390 pr_debug("Sending pending skb\n");
374 nfc_data_exchange(local->dev, local->target_idx, 391 nfc_data_exchange(local->dev, local->target_idx,
375 skb, nfc_llcp_recv, local); 392 skb, nfc_llcp_recv, local);
376 } else { 393 } else {
377 nfc_llcp_send_symm(local->dev); 394 nfc_llcp_send_symm(local->dev);
378 } 395 }
379 396
380 mod_timer(&local->link_timer, 397 mod_timer(&local->link_timer,
381 jiffies + msecs_to_jiffies(local->remote_lto)); 398 jiffies + msecs_to_jiffies(local->remote_lto));
382} 399}
383 400
384static u8 nfc_llcp_dsap(struct sk_buff *pdu) 401static u8 nfc_llcp_dsap(struct sk_buff *pdu)
@@ -408,13 +425,13 @@ static u8 nfc_llcp_nr(struct sk_buff *pdu)
408 425
409static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) 426static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
410{ 427{
411 pdu->data[2] = (sock->send_n << 4) | ((sock->recv_n - 1) % 16); 428 pdu->data[2] = (sock->send_n << 4) | (sock->recv_n % 16);
412 sock->send_n = (sock->send_n + 1) % 16; 429 sock->send_n = (sock->send_n + 1) % 16;
413 sock->recv_ack_n = (sock->recv_n - 1) % 16; 430 sock->recv_ack_n = (sock->recv_n - 1) % 16;
414} 431}
415 432
416static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, 433static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
417 u8 ssap, u8 dsap) 434 u8 ssap, u8 dsap)
418{ 435{
419 struct nfc_llcp_sock *sock, *llcp_sock, *n; 436 struct nfc_llcp_sock *sock, *llcp_sock, *n;
420 437
@@ -438,7 +455,7 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
438 455
439 list_for_each_entry_safe(llcp_sock, n, &sock->list, list) { 456 list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {
440 pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock, 457 pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
441 &llcp_sock->sk, llcp_sock->dsap); 458 &llcp_sock->sk, llcp_sock->dsap);
442 if (llcp_sock->dsap == dsap) { 459 if (llcp_sock->dsap == dsap) {
443 sock_hold(&llcp_sock->sk); 460 sock_hold(&llcp_sock->sk);
444 mutex_unlock(&local->socket_lock); 461 mutex_unlock(&local->socket_lock);
@@ -482,7 +499,7 @@ static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
482} 499}
483 500
484static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, 501static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
485 struct sk_buff *skb) 502 struct sk_buff *skb)
486{ 503{
487 struct sock *new_sk, *parent; 504 struct sock *new_sk, *parent;
488 struct nfc_llcp_sock *sock, *new_sock; 505 struct nfc_llcp_sock *sock, *new_sock;
@@ -494,7 +511,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
494 pr_debug("%d %d\n", dsap, ssap); 511 pr_debug("%d %d\n", dsap, ssap);
495 512
496 nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], 513 nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
497 skb->len - LLCP_HEADER_SIZE); 514 skb->len - LLCP_HEADER_SIZE);
498 515
499 if (dsap != LLCP_SAP_SDP) { 516 if (dsap != LLCP_SAP_SDP) {
500 bound_sap = dsap; 517 bound_sap = dsap;
@@ -513,7 +530,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
513 lock_sock(&sock->sk); 530 lock_sock(&sock->sk);
514 531
515 if (sock->dsap == LLCP_SAP_SDP && 532 if (sock->dsap == LLCP_SAP_SDP &&
516 sock->sk.sk_state == LLCP_LISTEN) 533 sock->sk.sk_state == LLCP_LISTEN)
517 goto enqueue; 534 goto enqueue;
518 } else { 535 } else {
519 u8 *sn; 536 u8 *sn;
@@ -529,23 +546,23 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
529 546
530 mutex_lock(&local->socket_lock); 547 mutex_lock(&local->socket_lock);
531 for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET; 548 for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
532 bound_sap++) { 549 bound_sap++) {
533 sock = local->sockets[bound_sap]; 550 sock = local->sockets[bound_sap];
534 if (sock == NULL) 551 if (sock == NULL)
535 continue; 552 continue;
536 553
537 if (sock->service_name == NULL || 554 if (sock->service_name == NULL ||
538 sock->service_name_len == 0) 555 sock->service_name_len == 0)
539 continue; 556 continue;
540 557
541 if (sock->service_name_len != sn_len) 558 if (sock->service_name_len != sn_len)
542 continue; 559 continue;
543 560
544 if (sock->dsap == LLCP_SAP_SDP && 561 if (sock->dsap == LLCP_SAP_SDP &&
545 sock->sk.sk_state == LLCP_LISTEN && 562 sock->sk.sk_state == LLCP_LISTEN &&
546 !memcmp(sn, sock->service_name, sn_len)) { 563 !memcmp(sn, sock->service_name, sn_len)) {
547 pr_debug("Found service name at SAP %d\n", 564 pr_debug("Found service name at SAP %d\n",
548 bound_sap); 565 bound_sap);
549 sock_hold(&sock->sk); 566 sock_hold(&sock->sk);
550 mutex_unlock(&local->socket_lock); 567 mutex_unlock(&local->socket_lock);
551 568
@@ -570,8 +587,7 @@ enqueue:
570 goto fail; 587 goto fail;
571 } 588 }
572 589
573 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, 590 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
574 GFP_ATOMIC);
575 if (new_sk == NULL) { 591 if (new_sk == NULL) {
576 reason = LLCP_DM_REJ; 592 reason = LLCP_DM_REJ;
577 release_sock(&sock->sk); 593 release_sock(&sock->sk);
@@ -616,8 +632,39 @@ fail:
616 632
617} 633}
618 634
635int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
636{
637 int nr_frames = 0;
638 struct nfc_llcp_local *local = sock->local;
639
640 pr_debug("Remote ready %d tx queue len %d remote rw %d",
641 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
642 local->remote_rw);
643
644 /* Try to queue some I frames for transmission */
645 while (sock->remote_ready &&
646 skb_queue_len(&sock->tx_pending_queue) < local->remote_rw) {
647 struct sk_buff *pdu, *pending_pdu;
648
649 pdu = skb_dequeue(&sock->tx_queue);
650 if (pdu == NULL)
651 break;
652
653 /* Update N(S)/N(R) */
654 nfc_llcp_set_nrns(sock, pdu);
655
656 pending_pdu = skb_clone(pdu, GFP_KERNEL);
657
658 skb_queue_tail(&local->tx_queue, pdu);
659 skb_queue_tail(&sock->tx_pending_queue, pending_pdu);
660 nr_frames++;
661 }
662
663 return nr_frames;
664}
665
619static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, 666static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
620 struct sk_buff *skb) 667 struct sk_buff *skb)
621{ 668{
622 struct nfc_llcp_sock *llcp_sock; 669 struct nfc_llcp_sock *llcp_sock;
623 struct sock *sk; 670 struct sock *sk;
@@ -644,15 +691,15 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
644 nfc_llcp_sock_put(llcp_sock); 691 nfc_llcp_sock_put(llcp_sock);
645 } 692 }
646 693
647 if (ns == llcp_sock->recv_n)
648 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
649 else
650 pr_err("Received out of sequence I PDU\n");
651
652 /* Pass the payload upstream */ 694 /* Pass the payload upstream */
653 if (ptype == LLCP_PDU_I) { 695 if (ptype == LLCP_PDU_I) {
654 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); 696 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
655 697
698 if (ns == llcp_sock->recv_n)
699 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
700 else
701 pr_err("Received out of sequence I PDU\n");
702
656 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); 703 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
657 if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { 704 if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
658 pr_err("receive queue is full\n"); 705 pr_err("receive queue is full\n");
@@ -673,30 +720,20 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
673 } 720 }
674 } 721 }
675 722
676 /* Queue some I frames for transmission */ 723 if (ptype == LLCP_PDU_RR)
677 while (llcp_sock->remote_ready && 724 llcp_sock->remote_ready = true;
678 skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) { 725 else if (ptype == LLCP_PDU_RNR)
679 struct sk_buff *pdu, *pending_pdu; 726 llcp_sock->remote_ready = false;
680
681 pdu = skb_dequeue(&llcp_sock->tx_queue);
682 if (pdu == NULL)
683 break;
684
685 /* Update N(S)/N(R) */
686 nfc_llcp_set_nrns(llcp_sock, pdu);
687 727
688 pending_pdu = skb_clone(pdu, GFP_KERNEL); 728 if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
689 729 nfc_llcp_send_rr(llcp_sock);
690 skb_queue_tail(&local->tx_queue, pdu);
691 skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu);
692 }
693 730
694 release_sock(sk); 731 release_sock(sk);
695 nfc_llcp_sock_put(llcp_sock); 732 nfc_llcp_sock_put(llcp_sock);
696} 733}
697 734
698static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, 735static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
699 struct sk_buff *skb) 736 struct sk_buff *skb)
700{ 737{
701 struct nfc_llcp_sock *llcp_sock; 738 struct nfc_llcp_sock *llcp_sock;
702 struct sock *sk; 739 struct sock *sk;
@@ -718,7 +755,6 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
718 nfc_llcp_sock_put(llcp_sock); 755 nfc_llcp_sock_put(llcp_sock);
719 } 756 }
720 757
721
722 if (sk->sk_state == LLCP_CONNECTED) { 758 if (sk->sk_state == LLCP_CONNECTED) {
723 nfc_put_device(local->dev); 759 nfc_put_device(local->dev);
724 sk->sk_state = LLCP_CLOSED; 760 sk->sk_state = LLCP_CLOSED;
@@ -731,13 +767,11 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
731 nfc_llcp_sock_put(llcp_sock); 767 nfc_llcp_sock_put(llcp_sock);
732} 768}
733 769
734static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, 770static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
735 struct sk_buff *skb)
736{ 771{
737 struct nfc_llcp_sock *llcp_sock; 772 struct nfc_llcp_sock *llcp_sock;
738 u8 dsap, ssap; 773 u8 dsap, ssap;
739 774
740
741 dsap = nfc_llcp_dsap(skb); 775 dsap = nfc_llcp_dsap(skb);
742 ssap = nfc_llcp_ssap(skb); 776 ssap = nfc_llcp_ssap(skb);
743 777
@@ -756,7 +790,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
756 llcp_sock->dsap = ssap; 790 llcp_sock->dsap = ssap;
757 791
758 nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], 792 nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
759 skb->len - LLCP_HEADER_SIZE); 793 skb->len - LLCP_HEADER_SIZE);
760 794
761 nfc_llcp_sock_put(llcp_sock); 795 nfc_llcp_sock_put(llcp_sock);
762} 796}
@@ -764,7 +798,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
764static void nfc_llcp_rx_work(struct work_struct *work) 798static void nfc_llcp_rx_work(struct work_struct *work)
765{ 799{
766 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 800 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
767 rx_work); 801 rx_work);
768 u8 dsap, ssap, ptype; 802 u8 dsap, ssap, ptype;
769 struct sk_buff *skb; 803 struct sk_buff *skb;
770 804
@@ -802,6 +836,7 @@ static void nfc_llcp_rx_work(struct work_struct *work)
802 836
803 case LLCP_PDU_I: 837 case LLCP_PDU_I:
804 case LLCP_PDU_RR: 838 case LLCP_PDU_RR:
839 case LLCP_PDU_RNR:
805 pr_debug("I frame\n"); 840 pr_debug("I frame\n");
806 nfc_llcp_recv_hdlc(local, skb); 841 nfc_llcp_recv_hdlc(local, skb);
807 break; 842 break;
@@ -821,7 +856,7 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
821 856
822 pr_debug("Received an LLCP PDU\n"); 857 pr_debug("Received an LLCP PDU\n");
823 if (err < 0) { 858 if (err < 0) {
824 pr_err("err %d", err); 859 pr_err("err %d\n", err);
825 return; 860 return;
826 } 861 }
827 862
@@ -840,6 +875,8 @@ void nfc_llcp_mac_is_down(struct nfc_dev *dev)
840 if (local == NULL) 875 if (local == NULL)
841 return; 876 return;
842 877
878 nfc_llcp_clear_sdp(local);
879
843 /* Close and purge all existing sockets */ 880 /* Close and purge all existing sockets */
844 nfc_llcp_socket_release(local); 881 nfc_llcp_socket_release(local);
845} 882}
@@ -865,7 +902,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
865 queue_work(local->tx_wq, &local->tx_work); 902 queue_work(local->tx_wq, &local->tx_work);
866 } else { 903 } else {
867 mod_timer(&local->link_timer, 904 mod_timer(&local->link_timer,
868 jiffies + msecs_to_jiffies(local->remote_lto)); 905 jiffies + msecs_to_jiffies(local->remote_lto));
869 } 906 }
870} 907}
871 908
@@ -891,8 +928,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
891 skb_queue_head_init(&local->tx_queue); 928 skb_queue_head_init(&local->tx_queue);
892 INIT_WORK(&local->tx_work, nfc_llcp_tx_work); 929 INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
893 snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); 930 snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
894 local->tx_wq = alloc_workqueue(name, 931 local->tx_wq =
895 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); 932 alloc_workqueue(name,
933 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
934 1);
896 if (local->tx_wq == NULL) { 935 if (local->tx_wq == NULL) {
897 err = -ENOMEM; 936 err = -ENOMEM;
898 goto err_local; 937 goto err_local;
@@ -901,8 +940,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
901 local->rx_pending = NULL; 940 local->rx_pending = NULL;
902 INIT_WORK(&local->rx_work, nfc_llcp_rx_work); 941 INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
903 snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); 942 snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
904 local->rx_wq = alloc_workqueue(name, 943 local->rx_wq =
905 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); 944 alloc_workqueue(name,
945 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
946 1);
906 if (local->rx_wq == NULL) { 947 if (local->rx_wq == NULL) {
907 err = -ENOMEM; 948 err = -ENOMEM;
908 goto err_tx_wq; 949 goto err_tx_wq;
@@ -910,8 +951,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
910 951
911 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); 952 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
912 snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); 953 snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
913 local->timeout_wq = alloc_workqueue(name, 954 local->timeout_wq =
914 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); 955 alloc_workqueue(name,
956 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
957 1);
915 if (local->timeout_wq == NULL) { 958 if (local->timeout_wq == NULL) {
916 err = -ENOMEM; 959 err = -ENOMEM;
917 goto err_rx_wq; 960 goto err_rx_wq;
generated by cgit v1.2.2 (git 2.25.0) at 2024-10-12 01:25:06 -0400