diff options
Diffstat (limited to 'net/nfc/llcp/commands.c')
-rw-r--r-- | net/nfc/llcp/commands.c | 163 |
1 files changed, 147 insertions, 16 deletions
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index 151f2ef429c4..7b76eb7192f3 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c | |||
@@ -118,7 +118,7 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) | |||
118 | } | 118 | } |
119 | 119 | ||
120 | int nfc_llcp_parse_tlv(struct nfc_llcp_local *local, | 120 | int nfc_llcp_parse_tlv(struct nfc_llcp_local *local, |
121 | u8 *tlv_array, u16 tlv_array_len) | 121 | u8 *tlv_array, u16 tlv_array_len) |
122 | { | 122 | { |
123 | u8 *tlv = tlv_array, type, length, offset = 0; | 123 | u8 *tlv = tlv_array, type, length, offset = 0; |
124 | 124 | ||
@@ -152,6 +152,8 @@ int nfc_llcp_parse_tlv(struct nfc_llcp_local *local, | |||
152 | case LLCP_TLV_RW: | 152 | case LLCP_TLV_RW: |
153 | local->remote_rw = llcp_tlv_rw(tlv); | 153 | local->remote_rw = llcp_tlv_rw(tlv); |
154 | break; | 154 | break; |
155 | case LLCP_TLV_SN: | ||
156 | break; | ||
155 | default: | 157 | default: |
156 | pr_err("Invalid gt tlv value 0x%x\n", type); | 158 | pr_err("Invalid gt tlv value 0x%x\n", type); |
157 | break; | 159 | break; |
@@ -162,15 +164,15 @@ int nfc_llcp_parse_tlv(struct nfc_llcp_local *local, | |||
162 | } | 164 | } |
163 | 165 | ||
164 | pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n", | 166 | pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n", |
165 | local->remote_version, local->remote_miu, | 167 | local->remote_version, local->remote_miu, |
166 | local->remote_lto, local->remote_opt, | 168 | local->remote_lto, local->remote_opt, |
167 | local->remote_wks, local->remote_rw); | 169 | local->remote_wks, local->remote_rw); |
168 | 170 | ||
169 | return 0; | 171 | return 0; |
170 | } | 172 | } |
171 | 173 | ||
172 | static struct sk_buff *llcp_add_header(struct sk_buff *pdu, | 174 | static struct sk_buff *llcp_add_header(struct sk_buff *pdu, |
173 | u8 dsap, u8 ssap, u8 ptype) | 175 | u8 dsap, u8 ssap, u8 ptype) |
174 | { | 176 | { |
175 | u8 header[2]; | 177 | u8 header[2]; |
176 | 178 | ||
@@ -186,7 +188,8 @@ static struct sk_buff *llcp_add_header(struct sk_buff *pdu, | |||
186 | return pdu; | 188 | return pdu; |
187 | } | 189 | } |
188 | 190 | ||
189 | static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, u8 tlv_length) | 191 | static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, |
192 | u8 tlv_length) | ||
190 | { | 193 | { |
191 | /* XXX Add an skb length check */ | 194 | /* XXX Add an skb length check */ |
192 | 195 | ||
@@ -199,7 +202,7 @@ static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, u8 tlv_length) | |||
199 | } | 202 | } |
200 | 203 | ||
201 | static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, | 204 | static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, |
202 | u8 cmd, u16 size) | 205 | u8 cmd, u16 size) |
203 | { | 206 | { |
204 | struct sk_buff *skb; | 207 | struct sk_buff *skb; |
205 | int err; | 208 | int err; |
@@ -208,7 +211,7 @@ static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, | |||
208 | return NULL; | 211 | return NULL; |
209 | 212 | ||
210 | skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, | 213 | skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, |
211 | size + LLCP_HEADER_SIZE, &err); | 214 | size + LLCP_HEADER_SIZE, &err); |
212 | if (skb == NULL) { | 215 | if (skb == NULL) { |
213 | pr_err("Could not allocate PDU\n"); | 216 | pr_err("Could not allocate PDU\n"); |
214 | return NULL; | 217 | return NULL; |
@@ -276,7 +279,7 @@ int nfc_llcp_send_symm(struct nfc_dev *dev) | |||
276 | skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); | 279 | skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); |
277 | 280 | ||
278 | return nfc_data_exchange(dev, local->target_idx, skb, | 281 | return nfc_data_exchange(dev, local->target_idx, skb, |
279 | nfc_llcp_recv, local); | 282 | nfc_llcp_recv, local); |
280 | } | 283 | } |
281 | 284 | ||
282 | int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) | 285 | int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) |
@@ -284,6 +287,9 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) | |||
284 | struct nfc_llcp_local *local; | 287 | struct nfc_llcp_local *local; |
285 | struct sk_buff *skb; | 288 | struct sk_buff *skb; |
286 | u8 *service_name_tlv = NULL, service_name_tlv_length; | 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; | ||
292 | __be16 miux; | ||
287 | int err; | 293 | int err; |
288 | u16 size = 0; | 294 | u16 size = 0; |
289 | 295 | ||
@@ -295,12 +301,21 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) | |||
295 | 301 | ||
296 | if (sock->service_name != NULL) { | 302 | if (sock->service_name != NULL) { |
297 | service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN, | 303 | service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN, |
298 | sock->service_name, | 304 | sock->service_name, |
299 | sock->service_name_len, | 305 | sock->service_name_len, |
300 | &service_name_tlv_length); | 306 | &service_name_tlv_length); |
301 | size += service_name_tlv_length; | 307 | size += service_name_tlv_length; |
302 | } | 308 | } |
303 | 309 | ||
310 | miux = cpu_to_be16(LLCP_MAX_MIUX); | ||
311 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, | ||
312 | &miux_tlv_length); | ||
313 | size += miux_tlv_length; | ||
314 | |||
315 | rw = LLCP_MAX_RW; | ||
316 | rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); | ||
317 | size += rw_tlv_length; | ||
318 | |||
304 | pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); | 319 | pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); |
305 | 320 | ||
306 | skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); | 321 | skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); |
@@ -311,7 +326,10 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) | |||
311 | 326 | ||
312 | if (service_name_tlv != NULL) | 327 | if (service_name_tlv != NULL) |
313 | skb = llcp_add_tlv(skb, service_name_tlv, | 328 | skb = llcp_add_tlv(skb, service_name_tlv, |
314 | service_name_tlv_length); | 329 | service_name_tlv_length); |
330 | |||
331 | skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); | ||
332 | skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); | ||
315 | 333 | ||
316 | skb_queue_tail(&local->tx_queue, skb); | 334 | skb_queue_tail(&local->tx_queue, skb); |
317 | 335 | ||
@@ -321,6 +339,8 @@ error_tlv: | |||
321 | pr_err("error %d\n", err); | 339 | pr_err("error %d\n", err); |
322 | 340 | ||
323 | kfree(service_name_tlv); | 341 | kfree(service_name_tlv); |
342 | kfree(miux_tlv); | ||
343 | kfree(rw_tlv); | ||
324 | 344 | ||
325 | return err; | 345 | return err; |
326 | } | 346 | } |
@@ -329,6 +349,11 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) | |||
329 | { | 349 | { |
330 | struct nfc_llcp_local *local; | 350 | struct nfc_llcp_local *local; |
331 | struct sk_buff *skb; | 351 | struct sk_buff *skb; |
352 | u8 *miux_tlv = NULL, miux_tlv_length; | ||
353 | u8 *rw_tlv = NULL, rw_tlv_length, rw; | ||
354 | __be16 miux; | ||
355 | int err; | ||
356 | u16 size = 0; | ||
332 | 357 | ||
333 | pr_debug("Sending CC\n"); | 358 | pr_debug("Sending CC\n"); |
334 | 359 | ||
@@ -336,13 +361,35 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) | |||
336 | if (local == NULL) | 361 | if (local == NULL) |
337 | return -ENODEV; | 362 | return -ENODEV; |
338 | 363 | ||
339 | skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, 0); | 364 | miux = cpu_to_be16(LLCP_MAX_MIUX); |
340 | if (skb == NULL) | 365 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, |
341 | return -ENOMEM; | 366 | &miux_tlv_length); |
367 | size += miux_tlv_length; | ||
368 | |||
369 | rw = LLCP_MAX_RW; | ||
370 | rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); | ||
371 | size += rw_tlv_length; | ||
372 | |||
373 | skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size); | ||
374 | if (skb == NULL) { | ||
375 | err = -ENOMEM; | ||
376 | goto error_tlv; | ||
377 | } | ||
378 | |||
379 | skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); | ||
380 | skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); | ||
342 | 381 | ||
343 | skb_queue_tail(&local->tx_queue, skb); | 382 | skb_queue_tail(&local->tx_queue, skb); |
344 | 383 | ||
345 | return 0; | 384 | return 0; |
385 | |||
386 | error_tlv: | ||
387 | pr_err("error %d\n", err); | ||
388 | |||
389 | kfree(miux_tlv); | ||
390 | kfree(rw_tlv); | ||
391 | |||
392 | return err; | ||
346 | } | 393 | } |
347 | 394 | ||
348 | int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) | 395 | int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) |
@@ -397,3 +444,87 @@ int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock) | |||
397 | 444 | ||
398 | return 0; | 445 | return 0; |
399 | } | 446 | } |
447 | |||
448 | int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, | ||
449 | struct msghdr *msg, size_t len) | ||
450 | { | ||
451 | struct sk_buff *pdu; | ||
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; | ||
456 | |||
457 | pr_debug("Send I frame len %zd\n", len); | ||
458 | |||
459 | local = sock->local; | ||
460 | if (local == NULL) | ||
461 | return -ENODEV; | ||
462 | |||
463 | msg_data = kzalloc(len, GFP_KERNEL); | ||
464 | if (msg_data == NULL) | ||
465 | return -ENOMEM; | ||
466 | |||
467 | if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { | ||
468 | kfree(msg_data); | ||
469 | return -EFAULT; | ||
470 | } | ||
471 | |||
472 | remaining_len = len; | ||
473 | msg_ptr = msg_data; | ||
474 | |||
475 | while (remaining_len > 0) { | ||
476 | |||
477 | frag_len = min_t(u16, local->remote_miu, remaining_len); | ||
478 | |||
479 | pr_debug("Fragment %zd bytes remaining %zd", | ||
480 | frag_len, remaining_len); | ||
481 | |||
482 | pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, | ||
483 | frag_len + LLCP_SEQUENCE_SIZE); | ||
484 | if (pdu == NULL) | ||
485 | return -ENOMEM; | ||
486 | |||
487 | skb_put(pdu, LLCP_SEQUENCE_SIZE); | ||
488 | |||
489 | memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); | ||
490 | |||
491 | skb_queue_head(&sock->tx_queue, pdu); | ||
492 | |||
493 | lock_sock(sk); | ||
494 | |||
495 | nfc_llcp_queue_i_frames(sock); | ||
496 | |||
497 | release_sock(sk); | ||
498 | |||
499 | remaining_len -= frag_len; | ||
500 | msg_ptr += len; | ||
501 | } | ||
502 | |||
503 | kfree(msg_data); | ||
504 | |||
505 | return 0; | ||
506 | } | ||
507 | |||
508 | int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) | ||
509 | { | ||
510 | struct sk_buff *skb; | ||
511 | struct nfc_llcp_local *local; | ||
512 | |||
513 | pr_debug("Send rr nr %d\n", sock->recv_n); | ||
514 | |||
515 | local = sock->local; | ||
516 | if (local == NULL) | ||
517 | return -ENODEV; | ||
518 | |||
519 | skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); | ||
520 | if (skb == NULL) | ||
521 | return -ENOMEM; | ||
522 | |||
523 | skb_put(skb, LLCP_SEQUENCE_SIZE); | ||
524 | |||
525 | skb->data[2] = sock->recv_n % 16; | ||
526 | |||
527 | skb_queue_head(&local->tx_queue, skb); | ||
528 | |||
529 | return 0; | ||
530 | } | ||