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 u8
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
) & 0x7f;
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)
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_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 local
->remote_rw
= llcp_tlv_rw(tlv
);
158 pr_err("Invalid gt tlv value 0x%x\n", type
);
162 offset
+= length
+ 2;
166 pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n",
167 local
->remote_version
, local
->remote_miu
,
168 local
->remote_lto
, local
->remote_opt
,
169 local
->remote_wks
, local
->remote_rw
);
174 static struct sk_buff
*llcp_add_header(struct sk_buff
*pdu
,
175 u8 dsap
, u8 ssap
, u8 ptype
)
179 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype
, dsap
, ssap
);
181 header
[0] = (u8
)((dsap
<< 2) | (ptype
>> 2));
182 header
[1] = (u8
)((ptype
<< 6) | ssap
);
184 pr_debug("header 0x%x 0x%x\n", header
[0], header
[1]);
186 memcpy(skb_put(pdu
, LLCP_HEADER_SIZE
), header
, LLCP_HEADER_SIZE
);
191 static struct sk_buff
*llcp_add_tlv(struct sk_buff
*pdu
, u8
*tlv
,
194 /* XXX Add an skb length check */
199 memcpy(skb_put(pdu
, tlv_length
), tlv
, tlv_length
);
204 static struct sk_buff
*llcp_allocate_pdu(struct nfc_llcp_sock
*sock
,
213 skb
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
214 size
+ LLCP_HEADER_SIZE
, &err
);
216 pr_err("Could not allocate PDU\n");
220 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, cmd
);
225 int nfc_llcp_disconnect(struct nfc_llcp_sock
*sock
)
229 struct nfc_llcp_local
*local
;
232 pr_debug("Sending DISC\n");
242 size
+= LLCP_HEADER_SIZE
;
243 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
245 skb
= alloc_skb(size
, GFP_ATOMIC
);
249 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
251 skb
= llcp_add_header(skb
, sock
->ssap
, sock
->dsap
, LLCP_PDU_DISC
);
253 skb_queue_tail(&local
->tx_queue
, skb
);
258 int nfc_llcp_send_symm(struct nfc_dev
*dev
)
261 struct nfc_llcp_local
*local
;
264 pr_debug("Sending SYMM\n");
266 local
= nfc_llcp_find_local(dev
);
270 size
+= LLCP_HEADER_SIZE
;
271 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
273 skb
= alloc_skb(size
, GFP_KERNEL
);
277 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
279 skb
= llcp_add_header(skb
, 0, 0, LLCP_PDU_SYMM
);
281 return nfc_data_exchange(dev
, local
->target_idx
, skb
,
282 nfc_llcp_recv
, local
);
285 int nfc_llcp_send_connect(struct nfc_llcp_sock
*sock
)
287 struct nfc_llcp_local
*local
;
289 u8
*service_name_tlv
= NULL
, service_name_tlv_length
;
290 u8
*miux_tlv
= NULL
, miux_tlv_length
;
291 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
296 pr_debug("Sending CONNECT\n");
302 if (sock
->service_name
!= NULL
) {
303 service_name_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SN
,
305 sock
->service_name_len
,
306 &service_name_tlv_length
);
307 size
+= service_name_tlv_length
;
310 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
311 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
313 size
+= miux_tlv_length
;
316 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
317 size
+= rw_tlv_length
;
319 pr_debug("SKB size %d SN length %zu\n", size
, sock
->service_name_len
);
321 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CONNECT
, size
);
327 if (service_name_tlv
!= NULL
)
328 skb
= llcp_add_tlv(skb
, service_name_tlv
,
329 service_name_tlv_length
);
331 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
332 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
334 skb_queue_tail(&local
->tx_queue
, skb
);
339 pr_err("error %d\n", err
);
341 kfree(service_name_tlv
);
348 int nfc_llcp_send_cc(struct nfc_llcp_sock
*sock
)
350 struct nfc_llcp_local
*local
;
352 u8
*miux_tlv
= NULL
, miux_tlv_length
;
353 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
358 pr_debug("Sending CC\n");
364 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
365 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
367 size
+= miux_tlv_length
;
370 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
371 size
+= rw_tlv_length
;
373 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CC
, size
);
379 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
380 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
382 skb_queue_tail(&local
->tx_queue
, skb
);
387 pr_err("error %d\n", err
);
395 int nfc_llcp_send_dm(struct nfc_llcp_local
*local
, u8 ssap
, u8 dsap
, u8 reason
)
399 u16 size
= 1; /* Reason code */
401 pr_debug("Sending DM reason 0x%x\n", reason
);
410 size
+= LLCP_HEADER_SIZE
;
411 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
413 skb
= alloc_skb(size
, GFP_KERNEL
);
417 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
419 skb
= llcp_add_header(skb
, ssap
, dsap
, LLCP_PDU_DM
);
421 memcpy(skb_put(skb
, 1), &reason
, 1);
423 skb_queue_head(&local
->tx_queue
, skb
);
428 int nfc_llcp_send_disconnect(struct nfc_llcp_sock
*sock
)
431 struct nfc_llcp_local
*local
;
433 pr_debug("Send DISC\n");
439 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
443 skb_queue_head(&local
->tx_queue
, skb
);
448 int nfc_llcp_send_i_frame(struct nfc_llcp_sock
*sock
,
449 struct msghdr
*msg
, size_t len
)
452 struct sock
*sk
= &sock
->sk
;
453 struct nfc_llcp_local
*local
;
454 size_t frag_len
= 0, remaining_len
;
455 u8
*msg_data
, *msg_ptr
;
457 pr_debug("Send I frame len %zd\n", len
);
463 msg_data
= kzalloc(len
, GFP_KERNEL
);
464 if (msg_data
== NULL
)
467 if (memcpy_fromiovec(msg_data
, msg
->msg_iov
, len
)) {
475 while (remaining_len
> 0) {
477 frag_len
= min_t(size_t, local
->remote_miu
, remaining_len
);
479 pr_debug("Fragment %zd bytes remaining %zd",
480 frag_len
, remaining_len
);
482 pdu
= llcp_allocate_pdu(sock
, LLCP_PDU_I
,
483 frag_len
+ LLCP_SEQUENCE_SIZE
);
487 skb_put(pdu
, LLCP_SEQUENCE_SIZE
);
489 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
491 skb_queue_head(&sock
->tx_queue
, pdu
);
495 nfc_llcp_queue_i_frames(sock
);
499 remaining_len
-= frag_len
;
508 int nfc_llcp_send_rr(struct nfc_llcp_sock
*sock
)
511 struct nfc_llcp_local
*local
;
513 pr_debug("Send rr nr %d\n", sock
->recv_n
);
519 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_RR
, LLCP_SEQUENCE_SIZE
);
523 skb_put(skb
, LLCP_SEQUENCE_SIZE
);
525 skb
->data
[2] = sock
->recv_n
;
527 skb_queue_head(&local
->tx_queue
, skb
);
This page took 0.041051 seconds and 6 git commands to generate.