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, write to the
16 * Free Software Foundation, Inc.,
17 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
20 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
22 #include <linux/init.h>
23 #include <linux/kernel.h>
24 #include <linux/module.h>
25 #include <linux/nfc.h>
27 #include <net/nfc/nfc.h>
32 static u8 llcp_tlv_length
[LLCP_TLV_MAX
] = {
46 static u8
llcp_tlv8(u8
*tlv
, u8 type
)
48 if (tlv
[0] != type
|| tlv
[1] != llcp_tlv_length
[tlv
[0]])
54 static u16
llcp_tlv16(u8
*tlv
, u8 type
)
56 if (tlv
[0] != type
|| tlv
[1] != llcp_tlv_length
[tlv
[0]])
59 return be16_to_cpu(*((__be16
*)(tlv
+ 2)));
63 static u8
llcp_tlv_version(u8
*tlv
)
65 return llcp_tlv8(tlv
, LLCP_TLV_VERSION
);
68 static u16
llcp_tlv_miux(u8
*tlv
)
70 return llcp_tlv16(tlv
, LLCP_TLV_MIUX
) & 0x7ff;
73 static u16
llcp_tlv_wks(u8
*tlv
)
75 return llcp_tlv16(tlv
, LLCP_TLV_WKS
);
78 static u16
llcp_tlv_lto(u8
*tlv
)
80 return llcp_tlv8(tlv
, LLCP_TLV_LTO
);
83 static u8
llcp_tlv_opt(u8
*tlv
)
85 return llcp_tlv8(tlv
, LLCP_TLV_OPT
);
88 static u8
llcp_tlv_rw(u8
*tlv
)
90 return llcp_tlv8(tlv
, LLCP_TLV_RW
) & 0xf;
93 u8
*nfc_llcp_build_tlv(u8 type
, u8
*value
, u8 value_length
, u8
*tlv_length
)
97 pr_debug("type %d\n", type
);
99 if (type
>= LLCP_TLV_MAX
)
102 length
= llcp_tlv_length
[type
];
103 if (length
== 0 && value_length
== 0)
105 else if (length
== 0)
106 length
= value_length
;
108 *tlv_length
= 2 + length
;
109 tlv
= kzalloc(2 + length
, GFP_KERNEL
);
115 memcpy(tlv
+ 2, value
, length
);
120 int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local
*local
,
121 u8
*tlv_array
, u16 tlv_array_len
)
123 u8
*tlv
= tlv_array
, type
, length
, offset
= 0;
125 pr_debug("TLV array length %d\n", tlv_array_len
);
130 while (offset
< tlv_array_len
) {
134 pr_debug("type 0x%x length %d\n", type
, length
);
137 case LLCP_TLV_VERSION
:
138 local
->remote_version
= llcp_tlv_version(tlv
);
141 local
->remote_miu
= llcp_tlv_miux(tlv
) + 128;
144 local
->remote_wks
= llcp_tlv_wks(tlv
);
147 local
->remote_lto
= llcp_tlv_lto(tlv
) * 10;
150 local
->remote_opt
= llcp_tlv_opt(tlv
);
153 pr_err("Invalid gt tlv value 0x%x\n", type
);
157 offset
+= length
+ 2;
161 pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
162 local
->remote_version
, local
->remote_miu
,
163 local
->remote_lto
, local
->remote_opt
,
169 int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock
*sock
,
170 u8
*tlv_array
, u16 tlv_array_len
)
172 u8
*tlv
= tlv_array
, type
, length
, offset
= 0;
174 pr_debug("TLV array length %d\n", tlv_array_len
);
179 while (offset
< tlv_array_len
) {
183 pr_debug("type 0x%x length %d\n", type
, length
);
187 sock
->miu
= llcp_tlv_miux(tlv
) + 128;
190 sock
->rw
= llcp_tlv_rw(tlv
);
195 pr_err("Invalid gt tlv value 0x%x\n", type
);
199 offset
+= length
+ 2;
203 pr_debug("sock %p rw %d miu %d\n", sock
, sock
->rw
, sock
->miu
);
208 static struct sk_buff
*llcp_add_header(struct sk_buff
*pdu
,
209 u8 dsap
, u8 ssap
, u8 ptype
)
213 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype
, dsap
, ssap
);
215 header
[0] = (u8
)((dsap
<< 2) | (ptype
>> 2));
216 header
[1] = (u8
)((ptype
<< 6) | ssap
);
218 pr_debug("header 0x%x 0x%x\n", header
[0], header
[1]);
220 memcpy(skb_put(pdu
, LLCP_HEADER_SIZE
), header
, LLCP_HEADER_SIZE
);
225 static struct sk_buff
*llcp_add_tlv(struct sk_buff
*pdu
, u8
*tlv
,
228 /* XXX Add an skb length check */
233 memcpy(skb_put(pdu
, tlv_length
), tlv
, tlv_length
);
238 static struct sk_buff
*llcp_allocate_pdu(struct nfc_llcp_sock
*sock
,
247 skb
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
248 size
+ LLCP_HEADER_SIZE
, &err
);
250 pr_err("Could not allocate PDU\n");
254 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, cmd
);
259 int nfc_llcp_disconnect(struct nfc_llcp_sock
*sock
)
263 struct nfc_llcp_local
*local
;
265 pr_debug("Sending DISC\n");
275 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
279 skb_queue_tail(&local
->tx_queue
, skb
);
284 int nfc_llcp_send_symm(struct nfc_dev
*dev
)
287 struct nfc_llcp_local
*local
;
290 pr_debug("Sending SYMM\n");
292 local
= nfc_llcp_find_local(dev
);
296 size
+= LLCP_HEADER_SIZE
;
297 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
299 skb
= alloc_skb(size
, GFP_KERNEL
);
303 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
305 skb
= llcp_add_header(skb
, 0, 0, LLCP_PDU_SYMM
);
307 nfc_llcp_send_to_raw_sock(local
, skb
, NFC_LLCP_DIRECTION_TX
);
309 return nfc_data_exchange(dev
, local
->target_idx
, skb
,
310 nfc_llcp_recv
, local
);
313 int nfc_llcp_send_connect(struct nfc_llcp_sock
*sock
)
315 struct nfc_llcp_local
*local
;
317 u8
*service_name_tlv
= NULL
, service_name_tlv_length
;
318 u8
*miux_tlv
= NULL
, miux_tlv_length
;
319 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
324 pr_debug("Sending CONNECT\n");
330 if (sock
->service_name
!= NULL
) {
331 service_name_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SN
,
333 sock
->service_name_len
,
334 &service_name_tlv_length
);
335 size
+= service_name_tlv_length
;
338 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
339 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
341 size
+= miux_tlv_length
;
344 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
345 size
+= rw_tlv_length
;
347 pr_debug("SKB size %d SN length %zu\n", size
, sock
->service_name_len
);
349 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CONNECT
, size
);
355 if (service_name_tlv
!= NULL
)
356 skb
= llcp_add_tlv(skb
, service_name_tlv
,
357 service_name_tlv_length
);
359 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
360 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
362 skb_queue_tail(&local
->tx_queue
, skb
);
367 pr_err("error %d\n", err
);
369 kfree(service_name_tlv
);
376 int nfc_llcp_send_cc(struct nfc_llcp_sock
*sock
)
378 struct nfc_llcp_local
*local
;
380 u8
*miux_tlv
= NULL
, miux_tlv_length
;
381 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
386 pr_debug("Sending CC\n");
392 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
393 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
395 size
+= miux_tlv_length
;
398 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
399 size
+= rw_tlv_length
;
401 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CC
, size
);
407 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
408 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
410 skb_queue_tail(&local
->tx_queue
, skb
);
415 pr_err("error %d\n", err
);
423 int nfc_llcp_send_snl(struct nfc_llcp_local
*local
, u8 tid
, u8 sap
)
427 u8
*sdres_tlv
= NULL
, sdres_tlv_length
, sdres
[2];
430 pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid
, sap
);
441 sdres_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SDRES
, sdres
, 0,
443 if (sdres_tlv
== NULL
)
446 size
+= LLCP_HEADER_SIZE
;
447 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
448 size
+= sdres_tlv_length
;
450 skb
= alloc_skb(size
, GFP_KERNEL
);
456 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
458 skb
= llcp_add_header(skb
, LLCP_SAP_SDP
, LLCP_SAP_SDP
, LLCP_PDU_SNL
);
460 memcpy(skb_put(skb
, sdres_tlv_length
), sdres_tlv
, sdres_tlv_length
);
462 skb_queue_tail(&local
->tx_queue
, skb
);
469 int nfc_llcp_send_dm(struct nfc_llcp_local
*local
, u8 ssap
, u8 dsap
, u8 reason
)
473 u16 size
= 1; /* Reason code */
475 pr_debug("Sending DM reason 0x%x\n", reason
);
484 size
+= LLCP_HEADER_SIZE
;
485 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
487 skb
= alloc_skb(size
, GFP_KERNEL
);
491 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
493 skb
= llcp_add_header(skb
, dsap
, ssap
, LLCP_PDU_DM
);
495 memcpy(skb_put(skb
, 1), &reason
, 1);
497 skb_queue_head(&local
->tx_queue
, skb
);
502 int nfc_llcp_send_disconnect(struct nfc_llcp_sock
*sock
)
505 struct nfc_llcp_local
*local
;
507 pr_debug("Send DISC\n");
513 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
517 skb_queue_head(&local
->tx_queue
, skb
);
522 int nfc_llcp_send_i_frame(struct nfc_llcp_sock
*sock
,
523 struct msghdr
*msg
, size_t len
)
526 struct sock
*sk
= &sock
->sk
;
527 struct nfc_llcp_local
*local
;
528 size_t frag_len
= 0, remaining_len
;
529 u8
*msg_data
, *msg_ptr
;
531 pr_debug("Send I frame len %zd\n", len
);
537 msg_data
= kzalloc(len
, GFP_KERNEL
);
538 if (msg_data
== NULL
)
541 if (memcpy_fromiovec(msg_data
, msg
->msg_iov
, len
)) {
549 while (remaining_len
> 0) {
551 frag_len
= min_t(size_t, sock
->miu
, remaining_len
);
553 pr_debug("Fragment %zd bytes remaining %zd",
554 frag_len
, remaining_len
);
556 pdu
= llcp_allocate_pdu(sock
, LLCP_PDU_I
,
557 frag_len
+ LLCP_SEQUENCE_SIZE
);
561 skb_put(pdu
, LLCP_SEQUENCE_SIZE
);
563 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
565 skb_queue_tail(&sock
->tx_queue
, pdu
);
569 nfc_llcp_queue_i_frames(sock
);
573 remaining_len
-= frag_len
;
582 int nfc_llcp_send_rr(struct nfc_llcp_sock
*sock
)
585 struct nfc_llcp_local
*local
;
587 pr_debug("Send rr nr %d\n", sock
->recv_n
);
593 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_RR
, LLCP_SEQUENCE_SIZE
);
597 skb_put(skb
, LLCP_SEQUENCE_SIZE
);
599 skb
->data
[2] = sock
->recv_n
;
601 skb_queue_head(&local
->tx_queue
, skb
);
This page took 0.042013 seconds and 6 git commands to generate.