2 * Copyright (C) 2011 Intel Corporation. All rights reserved.
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.
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.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, see <http://www.gnu.org/licenses/>.
18 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
20 #include <linux/init.h>
21 #include <linux/kernel.h>
22 #include <linux/module.h>
23 #include <linux/nfc.h>
25 #include <net/nfc/nfc.h>
30 static u8 llcp_tlv_length
[LLCP_TLV_MAX
] = {
44 static u8
llcp_tlv8(u8
*tlv
, u8 type
)
46 if (tlv
[0] != type
|| tlv
[1] != llcp_tlv_length
[tlv
[0]])
52 static u16
llcp_tlv16(u8
*tlv
, u8 type
)
54 if (tlv
[0] != type
|| tlv
[1] != llcp_tlv_length
[tlv
[0]])
57 return be16_to_cpu(*((__be16
*)(tlv
+ 2)));
61 static u8
llcp_tlv_version(u8
*tlv
)
63 return llcp_tlv8(tlv
, LLCP_TLV_VERSION
);
66 static u16
llcp_tlv_miux(u8
*tlv
)
68 return llcp_tlv16(tlv
, LLCP_TLV_MIUX
) & 0x7ff;
71 static u16
llcp_tlv_wks(u8
*tlv
)
73 return llcp_tlv16(tlv
, LLCP_TLV_WKS
);
76 static u16
llcp_tlv_lto(u8
*tlv
)
78 return llcp_tlv8(tlv
, LLCP_TLV_LTO
);
81 static u8
llcp_tlv_opt(u8
*tlv
)
83 return llcp_tlv8(tlv
, LLCP_TLV_OPT
);
86 static u8
llcp_tlv_rw(u8
*tlv
)
88 return llcp_tlv8(tlv
, LLCP_TLV_RW
) & 0xf;
91 u8
*nfc_llcp_build_tlv(u8 type
, u8
*value
, u8 value_length
, u8
*tlv_length
)
95 pr_debug("type %d\n", type
);
97 if (type
>= LLCP_TLV_MAX
)
100 length
= llcp_tlv_length
[type
];
101 if (length
== 0 && value_length
== 0)
103 else if (length
== 0)
104 length
= value_length
;
106 *tlv_length
= 2 + length
;
107 tlv
= kzalloc(2 + length
, GFP_KERNEL
);
113 memcpy(tlv
+ 2, value
, length
);
118 struct nfc_llcp_sdp_tlv
*nfc_llcp_build_sdres_tlv(u8 tid
, u8 sap
)
120 struct nfc_llcp_sdp_tlv
*sdres
;
123 sdres
= kzalloc(sizeof(struct nfc_llcp_sdp_tlv
), GFP_KERNEL
);
130 sdres
->tlv
= nfc_llcp_build_tlv(LLCP_TLV_SDRES
, value
, 2,
132 if (sdres
->tlv
== NULL
) {
140 INIT_HLIST_NODE(&sdres
->node
);
145 struct nfc_llcp_sdp_tlv
*nfc_llcp_build_sdreq_tlv(u8 tid
, char *uri
,
148 struct nfc_llcp_sdp_tlv
*sdreq
;
150 pr_debug("uri: %s, len: %zu\n", uri
, uri_len
);
152 sdreq
= kzalloc(sizeof(struct nfc_llcp_sdp_tlv
), GFP_KERNEL
);
156 sdreq
->tlv_len
= uri_len
+ 3;
158 if (uri
[uri_len
- 1] == 0)
161 sdreq
->tlv
= kzalloc(sdreq
->tlv_len
+ 1, GFP_KERNEL
);
162 if (sdreq
->tlv
== NULL
) {
167 sdreq
->tlv
[0] = LLCP_TLV_SDREQ
;
168 sdreq
->tlv
[1] = sdreq
->tlv_len
- 2;
172 sdreq
->uri
= sdreq
->tlv
+ 3;
173 memcpy(sdreq
->uri
, uri
, uri_len
);
175 sdreq
->time
= jiffies
;
177 INIT_HLIST_NODE(&sdreq
->node
);
182 void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv
*sdp
)
188 void nfc_llcp_free_sdp_tlv_list(struct hlist_head
*head
)
190 struct nfc_llcp_sdp_tlv
*sdp
;
191 struct hlist_node
*n
;
193 hlist_for_each_entry_safe(sdp
, n
, head
, node
) {
194 hlist_del(&sdp
->node
);
196 nfc_llcp_free_sdp_tlv(sdp
);
200 int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local
*local
,
201 u8
*tlv_array
, u16 tlv_array_len
)
203 u8
*tlv
= tlv_array
, type
, length
, offset
= 0;
205 pr_debug("TLV array length %d\n", tlv_array_len
);
210 while (offset
< tlv_array_len
) {
214 pr_debug("type 0x%x length %d\n", type
, length
);
217 case LLCP_TLV_VERSION
:
218 local
->remote_version
= llcp_tlv_version(tlv
);
221 local
->remote_miu
= llcp_tlv_miux(tlv
) + 128;
224 local
->remote_wks
= llcp_tlv_wks(tlv
);
227 local
->remote_lto
= llcp_tlv_lto(tlv
) * 10;
230 local
->remote_opt
= llcp_tlv_opt(tlv
);
233 pr_err("Invalid gt tlv value 0x%x\n", type
);
237 offset
+= length
+ 2;
241 pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
242 local
->remote_version
, local
->remote_miu
,
243 local
->remote_lto
, local
->remote_opt
,
249 int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock
*sock
,
250 u8
*tlv_array
, u16 tlv_array_len
)
252 u8
*tlv
= tlv_array
, type
, length
, offset
= 0;
254 pr_debug("TLV array length %d\n", tlv_array_len
);
259 while (offset
< tlv_array_len
) {
263 pr_debug("type 0x%x length %d\n", type
, length
);
267 sock
->remote_miu
= llcp_tlv_miux(tlv
) + 128;
270 sock
->remote_rw
= llcp_tlv_rw(tlv
);
275 pr_err("Invalid gt tlv value 0x%x\n", type
);
279 offset
+= length
+ 2;
283 pr_debug("sock %p rw %d miu %d\n", sock
,
284 sock
->remote_rw
, sock
->remote_miu
);
289 static struct sk_buff
*llcp_add_header(struct sk_buff
*pdu
,
290 u8 dsap
, u8 ssap
, u8 ptype
)
294 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype
, dsap
, ssap
);
296 header
[0] = (u8
)((dsap
<< 2) | (ptype
>> 2));
297 header
[1] = (u8
)((ptype
<< 6) | ssap
);
299 pr_debug("header 0x%x 0x%x\n", header
[0], header
[1]);
301 memcpy(skb_put(pdu
, LLCP_HEADER_SIZE
), header
, LLCP_HEADER_SIZE
);
306 static struct sk_buff
*llcp_add_tlv(struct sk_buff
*pdu
, u8
*tlv
,
309 /* XXX Add an skb length check */
314 memcpy(skb_put(pdu
, tlv_length
), tlv
, tlv_length
);
319 static struct sk_buff
*llcp_allocate_pdu(struct nfc_llcp_sock
*sock
,
328 skb
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
329 size
+ LLCP_HEADER_SIZE
, &err
);
331 pr_err("Could not allocate PDU\n");
335 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, cmd
);
340 int nfc_llcp_send_disconnect(struct nfc_llcp_sock
*sock
)
344 struct nfc_llcp_local
*local
;
346 pr_debug("Sending DISC\n");
356 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
360 skb_queue_tail(&local
->tx_queue
, skb
);
365 int nfc_llcp_send_symm(struct nfc_dev
*dev
)
368 struct nfc_llcp_local
*local
;
371 pr_debug("Sending SYMM\n");
373 local
= nfc_llcp_find_local(dev
);
377 size
+= LLCP_HEADER_SIZE
;
378 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
380 skb
= alloc_skb(size
, GFP_KERNEL
);
384 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
386 skb
= llcp_add_header(skb
, 0, 0, LLCP_PDU_SYMM
);
388 __net_timestamp(skb
);
390 nfc_llcp_send_to_raw_sock(local
, skb
, NFC_DIRECTION_TX
);
392 return nfc_data_exchange(dev
, local
->target_idx
, skb
,
393 nfc_llcp_recv
, local
);
396 int nfc_llcp_send_connect(struct nfc_llcp_sock
*sock
)
398 struct nfc_llcp_local
*local
;
400 u8
*service_name_tlv
= NULL
, service_name_tlv_length
;
401 u8
*miux_tlv
= NULL
, miux_tlv_length
;
402 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
407 pr_debug("Sending CONNECT\n");
413 if (sock
->service_name
!= NULL
) {
414 service_name_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SN
,
416 sock
->service_name_len
,
417 &service_name_tlv_length
);
418 size
+= service_name_tlv_length
;
421 /* If the socket parameters are not set, use the local ones */
422 miux
= be16_to_cpu(sock
->miux
) > LLCP_MAX_MIUX
?
423 local
->miux
: sock
->miux
;
424 rw
= sock
->rw
> LLCP_MAX_RW
? local
->rw
: sock
->rw
;
426 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
428 size
+= miux_tlv_length
;
430 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
431 size
+= rw_tlv_length
;
433 pr_debug("SKB size %d SN length %zu\n", size
, sock
->service_name_len
);
435 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CONNECT
, size
);
441 if (service_name_tlv
!= NULL
)
442 skb
= llcp_add_tlv(skb
, service_name_tlv
,
443 service_name_tlv_length
);
445 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
446 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
448 skb_queue_tail(&local
->tx_queue
, skb
);
453 pr_err("error %d\n", err
);
455 kfree(service_name_tlv
);
462 int nfc_llcp_send_cc(struct nfc_llcp_sock
*sock
)
464 struct nfc_llcp_local
*local
;
466 u8
*miux_tlv
= NULL
, miux_tlv_length
;
467 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
472 pr_debug("Sending CC\n");
478 /* If the socket parameters are not set, use the local ones */
479 miux
= be16_to_cpu(sock
->miux
) > LLCP_MAX_MIUX
?
480 local
->miux
: sock
->miux
;
481 rw
= sock
->rw
> LLCP_MAX_RW
? local
->rw
: sock
->rw
;
483 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
485 size
+= miux_tlv_length
;
487 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
488 size
+= rw_tlv_length
;
490 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CC
, size
);
496 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
497 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
499 skb_queue_tail(&local
->tx_queue
, skb
);
504 pr_err("error %d\n", err
);
512 static struct sk_buff
*nfc_llcp_allocate_snl(struct nfc_llcp_local
*local
,
520 return ERR_PTR(-ENODEV
);
524 return ERR_PTR(-ENODEV
);
526 size
+= LLCP_HEADER_SIZE
;
527 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
530 skb
= alloc_skb(size
, GFP_KERNEL
);
532 return ERR_PTR(-ENOMEM
);
534 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
536 skb
= llcp_add_header(skb
, LLCP_SAP_SDP
, LLCP_SAP_SDP
, LLCP_PDU_SNL
);
541 int nfc_llcp_send_snl_sdres(struct nfc_llcp_local
*local
,
542 struct hlist_head
*tlv_list
, size_t tlvs_len
)
544 struct nfc_llcp_sdp_tlv
*sdp
;
545 struct hlist_node
*n
;
548 skb
= nfc_llcp_allocate_snl(local
, tlvs_len
);
552 hlist_for_each_entry_safe(sdp
, n
, tlv_list
, node
) {
553 memcpy(skb_put(skb
, sdp
->tlv_len
), sdp
->tlv
, sdp
->tlv_len
);
555 hlist_del(&sdp
->node
);
557 nfc_llcp_free_sdp_tlv(sdp
);
560 skb_queue_tail(&local
->tx_queue
, skb
);
565 int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local
*local
,
566 struct hlist_head
*tlv_list
, size_t tlvs_len
)
568 struct nfc_llcp_sdp_tlv
*sdreq
;
569 struct hlist_node
*n
;
572 skb
= nfc_llcp_allocate_snl(local
, tlvs_len
);
576 mutex_lock(&local
->sdreq_lock
);
578 if (hlist_empty(&local
->pending_sdreqs
))
579 mod_timer(&local
->sdreq_timer
,
580 jiffies
+ msecs_to_jiffies(3 * local
->remote_lto
));
582 hlist_for_each_entry_safe(sdreq
, n
, tlv_list
, node
) {
583 pr_debug("tid %d for %s\n", sdreq
->tid
, sdreq
->uri
);
585 memcpy(skb_put(skb
, sdreq
->tlv_len
), sdreq
->tlv
,
588 hlist_del(&sdreq
->node
);
590 hlist_add_head(&sdreq
->node
, &local
->pending_sdreqs
);
593 mutex_unlock(&local
->sdreq_lock
);
595 skb_queue_tail(&local
->tx_queue
, skb
);
600 int nfc_llcp_send_dm(struct nfc_llcp_local
*local
, u8 ssap
, u8 dsap
, u8 reason
)
604 u16 size
= 1; /* Reason code */
606 pr_debug("Sending DM reason 0x%x\n", reason
);
615 size
+= LLCP_HEADER_SIZE
;
616 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
618 skb
= alloc_skb(size
, GFP_KERNEL
);
622 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
624 skb
= llcp_add_header(skb
, dsap
, ssap
, LLCP_PDU_DM
);
626 memcpy(skb_put(skb
, 1), &reason
, 1);
628 skb_queue_head(&local
->tx_queue
, skb
);
633 int nfc_llcp_send_i_frame(struct nfc_llcp_sock
*sock
,
634 struct msghdr
*msg
, size_t len
)
637 struct sock
*sk
= &sock
->sk
;
638 struct nfc_llcp_local
*local
;
639 size_t frag_len
= 0, remaining_len
;
640 u8
*msg_data
, *msg_ptr
;
643 pr_debug("Send I frame len %zd\n", len
);
649 /* Remote is ready but has not acknowledged our frames */
650 if((sock
->remote_ready
&&
651 skb_queue_len(&sock
->tx_pending_queue
) >= sock
->remote_rw
&&
652 skb_queue_len(&sock
->tx_queue
) >= 2 * sock
->remote_rw
)) {
653 pr_err("Pending queue is full %d frames\n",
654 skb_queue_len(&sock
->tx_pending_queue
));
658 /* Remote is not ready and we've been queueing enough frames */
659 if ((!sock
->remote_ready
&&
660 skb_queue_len(&sock
->tx_queue
) >= 2 * sock
->remote_rw
)) {
661 pr_err("Tx queue is full %d frames\n",
662 skb_queue_len(&sock
->tx_queue
));
666 msg_data
= kzalloc(len
, GFP_KERNEL
);
667 if (msg_data
== NULL
)
670 if (memcpy_from_msg(msg_data
, msg
, len
)) {
679 remote_miu
= sock
->remote_miu
> LLCP_MAX_MIU
?
680 LLCP_DEFAULT_MIU
: sock
->remote_miu
;
682 frag_len
= min_t(size_t, remote_miu
, remaining_len
);
684 pr_debug("Fragment %zd bytes remaining %zd",
685 frag_len
, remaining_len
);
687 pdu
= llcp_allocate_pdu(sock
, LLCP_PDU_I
,
688 frag_len
+ LLCP_SEQUENCE_SIZE
);
694 skb_put(pdu
, LLCP_SEQUENCE_SIZE
);
696 if (likely(frag_len
> 0))
697 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
699 skb_queue_tail(&sock
->tx_queue
, pdu
);
703 nfc_llcp_queue_i_frames(sock
);
707 remaining_len
-= frag_len
;
709 } while (remaining_len
> 0);
716 int nfc_llcp_send_ui_frame(struct nfc_llcp_sock
*sock
, u8 ssap
, u8 dsap
,
717 struct msghdr
*msg
, size_t len
)
720 struct nfc_llcp_local
*local
;
721 size_t frag_len
= 0, remaining_len
;
722 u8
*msg_ptr
, *msg_data
;
726 pr_debug("Send UI frame len %zd\n", len
);
732 msg_data
= kzalloc(len
, GFP_KERNEL
);
733 if (msg_data
== NULL
)
736 if (memcpy_from_msg(msg_data
, msg
, len
)) {
745 remote_miu
= sock
->remote_miu
> LLCP_MAX_MIU
?
746 local
->remote_miu
: sock
->remote_miu
;
748 frag_len
= min_t(size_t, remote_miu
, remaining_len
);
750 pr_debug("Fragment %zd bytes remaining %zd",
751 frag_len
, remaining_len
);
753 pdu
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
754 frag_len
+ LLCP_HEADER_SIZE
, &err
);
756 pr_err("Could not allocate PDU\n");
760 pdu
= llcp_add_header(pdu
, dsap
, ssap
, LLCP_PDU_UI
);
762 if (likely(frag_len
> 0))
763 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
765 /* No need to check for the peer RW for UI frames */
766 skb_queue_tail(&local
->tx_queue
, pdu
);
768 remaining_len
-= frag_len
;
770 } while (remaining_len
> 0);
777 int nfc_llcp_send_rr(struct nfc_llcp_sock
*sock
)
780 struct nfc_llcp_local
*local
;
782 pr_debug("Send rr nr %d\n", sock
->recv_n
);
788 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_RR
, LLCP_SEQUENCE_SIZE
);
792 skb_put(skb
, LLCP_SEQUENCE_SIZE
);
794 skb
->data
[2] = sock
->recv_n
;
796 skb_queue_head(&local
->tx_queue
, skb
);
This page took 0.061317 seconds and 6 git commands to generate.