net/nfc/llcp_commands.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Copyright (C) 2011  Intel Corporation. All rights reserved.
0004  */
0005
0006 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
0007
0008 #include <linux/init.h>
0009 #include <linux/kernel.h>
0010 #include <linux/module.h>
0011 #include <linux/nfc.h>
0012
0013 #include <net/nfc/nfc.h>
0014
0015 #include "nfc.h"
0016 #include "llcp.h"
0017
0018 static const u8 llcp_tlv_length[LLCP_TLV_MAX] = {
0019     0,
0020     1, /* VERSION */
0021     2, /* MIUX */
0022     2, /* WKS */
0023     1, /* LTO */
0024     1, /* RW */
0025     0, /* SN */
0026     1, /* OPT */
0027     0, /* SDREQ */
0028     2, /* SDRES */
0029
0030 };
0031
0032 static u8 llcp_tlv8(const u8 *tlv, u8 type)
0033 {
0034     if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
0035         return 0;
0036
0037     return tlv[2];
0038 }
0039
0040 static u16 llcp_tlv16(const u8 *tlv, u8 type)
0041 {
0042     if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
0043         return 0;
0044
0045     return be16_to_cpu(*((__be16 *)(tlv + 2)));
0046 }
0047
0048
0049 static u8 llcp_tlv_version(const u8 *tlv)
0050 {
0051     return llcp_tlv8(tlv, LLCP_TLV_VERSION);
0052 }
0053
0054 static u16 llcp_tlv_miux(const u8 *tlv)
0055 {
0056     return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
0057 }
0058
0059 static u16 llcp_tlv_wks(const u8 *tlv)
0060 {
0061     return llcp_tlv16(tlv, LLCP_TLV_WKS);
0062 }
0063
0064 static u16 llcp_tlv_lto(const u8 *tlv)
0065 {
0066     return llcp_tlv8(tlv, LLCP_TLV_LTO);
0067 }
0068
0069 static u8 llcp_tlv_opt(const u8 *tlv)
0070 {
0071     return llcp_tlv8(tlv, LLCP_TLV_OPT);
0072 }
0073
0074 static u8 llcp_tlv_rw(const u8 *tlv)
0075 {
0076     return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
0077 }
0078
0079 u8 *nfc_llcp_build_tlv(u8 type, const u8 *value, u8 value_length, u8 *tlv_length)
0080 {
0081     u8 *tlv, length;
0082
0083     pr_debug("type %d\n", type);
0084
0085     if (type >= LLCP_TLV_MAX)
0086         return NULL;
0087
0088     length = llcp_tlv_length[type];
0089     if (length == 0 && value_length == 0)
0090         return NULL;
0091     else if (length == 0)
0092         length = value_length;
0093
0094     *tlv_length = 2 + length;
0095     tlv = kzalloc(2 + length, GFP_KERNEL);
0096     if (tlv == NULL)
0097         return tlv;
0098
0099     tlv[0] = type;
0100     tlv[1] = length;
0101     memcpy(tlv + 2, value, length);
0102
0103     return tlv;
0104 }
0105
0106 struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
0107 {
0108     struct nfc_llcp_sdp_tlv *sdres;
0109     u8 value[2];
0110
0111     sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
0112     if (sdres == NULL)
0113         return NULL;
0114
0115     value[0] = tid;
0116     value[1] = sap;
0117
0118     sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
0119                     &sdres->tlv_len);
0120     if (sdres->tlv == NULL) {
0121         kfree(sdres);
0122         return NULL;
0123     }
0124
0125     sdres->tid = tid;
0126     sdres->sap = sap;
0127
0128     INIT_HLIST_NODE(&sdres->node);
0129
0130     return sdres;
0131 }
0132
0133 struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, const char *uri,
0134                           size_t uri_len)
0135 {
0136     struct nfc_llcp_sdp_tlv *sdreq;
0137
0138     pr_debug("uri: %s, len: %zu\n", uri, uri_len);
0139
0140     /* sdreq->tlv_len is u8, takes uri_len, + 3 for header, + 1 for NULL */
0141     if (WARN_ON_ONCE(uri_len > U8_MAX - 4))
0142         return NULL;
0143
0144     sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
0145     if (sdreq == NULL)
0146         return NULL;
0147
0148     sdreq->tlv_len = uri_len + 3;
0149
0150     if (uri[uri_len - 1] == 0)
0151         sdreq->tlv_len--;
0152
0153     sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
0154     if (sdreq->tlv == NULL) {
0155         kfree(sdreq);
0156         return NULL;
0157     }
0158
0159     sdreq->tlv[0] = LLCP_TLV_SDREQ;
0160     sdreq->tlv[1] = sdreq->tlv_len - 2;
0161     sdreq->tlv[2] = tid;
0162
0163     sdreq->tid = tid;
0164     sdreq->uri = sdreq->tlv + 3;
0165     memcpy(sdreq->uri, uri, uri_len);
0166
0167     sdreq->time = jiffies;
0168
0169     INIT_HLIST_NODE(&sdreq->node);
0170
0171     return sdreq;
0172 }
0173
0174 void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
0175 {
0176     kfree(sdp->tlv);
0177     kfree(sdp);
0178 }
0179
0180 void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
0181 {
0182     struct nfc_llcp_sdp_tlv *sdp;
0183     struct hlist_node *n;
0184
0185     hlist_for_each_entry_safe(sdp, n, head, node) {
0186         hlist_del(&sdp->node);
0187
0188         nfc_llcp_free_sdp_tlv(sdp);
0189     }
0190 }
0191
0192 int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
0193               const u8 *tlv_array, u16 tlv_array_len)
0194 {
0195     const u8 *tlv = tlv_array;
0196     u8 type, length, offset = 0;
0197
0198     pr_debug("TLV array length %d\n", tlv_array_len);
0199
0200     if (local == NULL)
0201         return -ENODEV;
0202
0203     while (offset < tlv_array_len) {
0204         type = tlv[0];
0205         length = tlv[1];
0206
0207         pr_debug("type 0x%x length %d\n", type, length);
0208
0209         switch (type) {
0210         case LLCP_TLV_VERSION:
0211             local->remote_version = llcp_tlv_version(tlv);
0212             break;
0213         case LLCP_TLV_MIUX:
0214             local->remote_miu = llcp_tlv_miux(tlv) + 128;
0215             break;
0216         case LLCP_TLV_WKS:
0217             local->remote_wks = llcp_tlv_wks(tlv);
0218             break;
0219         case LLCP_TLV_LTO:
0220             local->remote_lto = llcp_tlv_lto(tlv) * 10;
0221             break;
0222         case LLCP_TLV_OPT:
0223             local->remote_opt = llcp_tlv_opt(tlv);
0224             break;
0225         default:
0226             pr_err("Invalid gt tlv value 0x%x\n", type);
0227             break;
0228         }
0229
0230         offset += length + 2;
0231         tlv += length + 2;
0232     }
0233
0234     pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
0235          local->remote_version, local->remote_miu,
0236          local->remote_lto, local->remote_opt,
0237          local->remote_wks);
0238
0239     return 0;
0240 }
0241
0242 int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
0243                   const u8 *tlv_array, u16 tlv_array_len)
0244 {
0245     const u8 *tlv = tlv_array;
0246     u8 type, length, offset = 0;
0247
0248     pr_debug("TLV array length %d\n", tlv_array_len);
0249
0250     if (sock == NULL)
0251         return -ENOTCONN;
0252
0253     while (offset < tlv_array_len) {
0254         type = tlv[0];
0255         length = tlv[1];
0256
0257         pr_debug("type 0x%x length %d\n", type, length);
0258
0259         switch (type) {
0260         case LLCP_TLV_MIUX:
0261             sock->remote_miu = llcp_tlv_miux(tlv) + 128;
0262             break;
0263         case LLCP_TLV_RW:
0264             sock->remote_rw = llcp_tlv_rw(tlv);
0265             break;
0266         case LLCP_TLV_SN:
0267             break;
0268         default:
0269             pr_err("Invalid gt tlv value 0x%x\n", type);
0270             break;
0271         }
0272
0273         offset += length + 2;
0274         tlv += length + 2;
0275     }
0276
0277     pr_debug("sock %p rw %d miu %d\n", sock,
0278          sock->remote_rw, sock->remote_miu);
0279
0280     return 0;
0281 }
0282
0283 static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
0284                        u8 dsap, u8 ssap, u8 ptype)
0285 {
0286     u8 header[2];
0287
0288     pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
0289
0290     header[0] = (u8)((dsap << 2) | (ptype >> 2));
0291     header[1] = (u8)((ptype << 6) | ssap);
0292
0293     pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
0294
0295     skb_put_data(pdu, header, LLCP_HEADER_SIZE);
0296
0297     return pdu;
0298 }
0299
0300 static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, const u8 *tlv,
0301                     u8 tlv_length)
0302 {
0303     /* XXX Add an skb length check */
0304
0305     if (tlv == NULL)
0306         return NULL;
0307
0308     skb_put_data(pdu, tlv, tlv_length);
0309
0310     return pdu;
0311 }
0312
0313 static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
0314                      u8 cmd, u16 size)
0315 {
0316     struct sk_buff *skb;
0317     int err;
0318
0319     if (sock->ssap == 0)
0320         return NULL;
0321
0322     skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
0323                  size + LLCP_HEADER_SIZE, &err);
0324     if (skb == NULL) {
0325         pr_err("Could not allocate PDU\n");
0326         return NULL;
0327     }
0328
0329     skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
0330
0331     return skb;
0332 }
0333
0334 int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
0335 {
0336     struct sk_buff *skb;
0337     struct nfc_dev *dev;
0338     struct nfc_llcp_local *local;
0339
0340     local = sock->local;
0341     if (local == NULL)
0342         return -ENODEV;
0343
0344     dev = sock->dev;
0345     if (dev == NULL)
0346         return -ENODEV;
0347
0348     skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
0349     if (skb == NULL)
0350         return -ENOMEM;
0351
0352     skb_queue_tail(&local->tx_queue, skb);
0353
0354     return 0;
0355 }
0356
0357 int nfc_llcp_send_symm(struct nfc_dev *dev)
0358 {
0359     struct sk_buff *skb;
0360     struct nfc_llcp_local *local;
0361     u16 size = 0;
0362
0363     local = nfc_llcp_find_local(dev);
0364     if (local == NULL)
0365         return -ENODEV;
0366
0367     size += LLCP_HEADER_SIZE;
0368     size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
0369
0370     skb = alloc_skb(size, GFP_KERNEL);
0371     if (skb == NULL)
0372         return -ENOMEM;
0373
0374     skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
0375
0376     skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
0377
0378     __net_timestamp(skb);
0379
0380     nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_TX);
0381
0382     return nfc_data_exchange(dev, local->target_idx, skb,
0383                  nfc_llcp_recv, local);
0384 }
0385
0386 int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
0387 {
0388     struct nfc_llcp_local *local;
0389     struct sk_buff *skb;
0390     const u8 *service_name_tlv = NULL;
0391     const u8 *miux_tlv = NULL;
0392     const u8 *rw_tlv = NULL;
0393     u8 service_name_tlv_length, miux_tlv_length,  rw_tlv_length, rw;
0394     int err;
0395     u16 size = 0;
0396     __be16 miux;
0397
0398     local = sock->local;
0399     if (local == NULL)
0400         return -ENODEV;
0401
0402     if (sock->service_name != NULL) {
0403         service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
0404                               sock->service_name,
0405                               sock->service_name_len,
0406                               &service_name_tlv_length);
0407         if (!service_name_tlv) {
0408             err = -ENOMEM;
0409             goto error_tlv;
0410         }
0411         size += service_name_tlv_length;
0412     }
0413
0414     /* If the socket parameters are not set, use the local ones */
0415     miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
0416         local->miux : sock->miux;
0417     rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
0418
0419     miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
0420                       &miux_tlv_length);
0421     if (!miux_tlv) {
0422         err = -ENOMEM;
0423         goto error_tlv;
0424     }
0425     size += miux_tlv_length;
0426
0427     rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
0428     if (!rw_tlv) {
0429         err = -ENOMEM;
0430         goto error_tlv;
0431     }
0432     size += rw_tlv_length;
0433
0434     pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
0435
0436     skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
0437     if (skb == NULL) {
0438         err = -ENOMEM;
0439         goto error_tlv;
0440     }
0441
0442     llcp_add_tlv(skb, service_name_tlv, service_name_tlv_length);
0443     llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
0444     llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
0445
0446     skb_queue_tail(&local->tx_queue, skb);
0447
0448     err = 0;
0449
0450 error_tlv:
0451     if (err)
0452         pr_err("error %d\n", err);
0453
0454     kfree(service_name_tlv);
0455     kfree(miux_tlv);
0456     kfree(rw_tlv);
0457
0458     return err;
0459 }
0460
0461 int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
0462 {
0463     struct nfc_llcp_local *local;
0464     struct sk_buff *skb;
0465     const u8 *miux_tlv = NULL;
0466     const u8 *rw_tlv = NULL;
0467     u8 miux_tlv_length, rw_tlv_length, rw;
0468     int err;
0469     u16 size = 0;
0470     __be16 miux;
0471
0472     local = sock->local;
0473     if (local == NULL)
0474         return -ENODEV;
0475
0476     /* If the socket parameters are not set, use the local ones */
0477     miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
0478         local->miux : sock->miux;
0479     rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
0480
0481     miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
0482                       &miux_tlv_length);
0483     if (!miux_tlv) {
0484         err = -ENOMEM;
0485         goto error_tlv;
0486     }
0487     size += miux_tlv_length;
0488
0489     rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
0490     if (!rw_tlv) {
0491         err = -ENOMEM;
0492         goto error_tlv;
0493     }
0494     size += rw_tlv_length;
0495
0496     skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
0497     if (skb == NULL) {
0498         err = -ENOMEM;
0499         goto error_tlv;
0500     }
0501
0502     llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
0503     llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
0504
0505     skb_queue_tail(&local->tx_queue, skb);
0506
0507     err = 0;
0508
0509 error_tlv:
0510     if (err)
0511         pr_err("error %d\n", err);
0512
0513     kfree(miux_tlv);
0514     kfree(rw_tlv);
0515
0516     return err;
0517 }
0518
0519 static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
0520                          size_t tlv_length)
0521 {
0522     struct sk_buff *skb;
0523     struct nfc_dev *dev;
0524     u16 size = 0;
0525
0526     if (local == NULL)
0527         return ERR_PTR(-ENODEV);
0528
0529     dev = local->dev;
0530     if (dev == NULL)
0531         return ERR_PTR(-ENODEV);
0532
0533     size += LLCP_HEADER_SIZE;
0534     size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
0535     size += tlv_length;
0536
0537     skb = alloc_skb(size, GFP_KERNEL);
0538     if (skb == NULL)
0539         return ERR_PTR(-ENOMEM);
0540
0541     skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
0542
0543     skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
0544
0545     return skb;
0546 }
0547
0548 int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
0549                 struct hlist_head *tlv_list, size_t tlvs_len)
0550 {
0551     struct nfc_llcp_sdp_tlv *sdp;
0552     struct hlist_node *n;
0553     struct sk_buff *skb;
0554
0555     skb = nfc_llcp_allocate_snl(local, tlvs_len);
0556     if (IS_ERR(skb))
0557         return PTR_ERR(skb);
0558
0559     hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
0560         skb_put_data(skb, sdp->tlv, sdp->tlv_len);
0561
0562         hlist_del(&sdp->node);
0563
0564         nfc_llcp_free_sdp_tlv(sdp);
0565     }
0566
0567     skb_queue_tail(&local->tx_queue, skb);
0568
0569     return 0;
0570 }
0571
0572 int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
0573                 struct hlist_head *tlv_list, size_t tlvs_len)
0574 {
0575     struct nfc_llcp_sdp_tlv *sdreq;
0576     struct hlist_node *n;
0577     struct sk_buff *skb;
0578
0579     skb = nfc_llcp_allocate_snl(local, tlvs_len);
0580     if (IS_ERR(skb))
0581         return PTR_ERR(skb);
0582
0583     mutex_lock(&local->sdreq_lock);
0584
0585     if (hlist_empty(&local->pending_sdreqs))
0586         mod_timer(&local->sdreq_timer,
0587               jiffies + msecs_to_jiffies(3 * local->remote_lto));
0588
0589     hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
0590         pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
0591
0592         skb_put_data(skb, sdreq->tlv, sdreq->tlv_len);
0593
0594         hlist_del(&sdreq->node);
0595
0596         hlist_add_head(&sdreq->node, &local->pending_sdreqs);
0597     }
0598
0599     mutex_unlock(&local->sdreq_lock);
0600
0601     skb_queue_tail(&local->tx_queue, skb);
0602
0603     return 0;
0604 }
0605
0606 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
0607 {
0608     struct sk_buff *skb;
0609     struct nfc_dev *dev;
0610     u16 size = 1; /* Reason code */
0611
0612     pr_debug("Sending DM reason 0x%x\n", reason);
0613
0614     if (local == NULL)
0615         return -ENODEV;
0616
0617     dev = local->dev;
0618     if (dev == NULL)
0619         return -ENODEV;
0620
0621     size += LLCP_HEADER_SIZE;
0622     size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
0623
0624     skb = alloc_skb(size, GFP_KERNEL);
0625     if (skb == NULL)
0626         return -ENOMEM;
0627
0628     skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
0629
0630     skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM);
0631
0632     skb_put_data(skb, &reason, 1);
0633
0634     skb_queue_head(&local->tx_queue, skb);
0635
0636     return 0;
0637 }
0638
0639 int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
0640               struct msghdr *msg, size_t len)
0641 {
0642     struct sk_buff *pdu;
0643     struct sock *sk = &sock->sk;
0644     struct nfc_llcp_local *local;
0645     size_t frag_len = 0, remaining_len;
0646     u8 *msg_data, *msg_ptr;
0647     u16 remote_miu;
0648
0649     pr_debug("Send I frame len %zd\n", len);
0650
0651     local = sock->local;
0652     if (local == NULL)
0653         return -ENODEV;
0654
0655     /* Remote is ready but has not acknowledged our frames */
0656     if((sock->remote_ready &&
0657         skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw &&
0658         skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
0659         pr_err("Pending queue is full %d frames\n",
0660                skb_queue_len(&sock->tx_pending_queue));
0661         return -ENOBUFS;
0662     }
0663
0664     /* Remote is not ready and we've been queueing enough frames */
0665     if ((!sock->remote_ready &&
0666          skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
0667         pr_err("Tx queue is full %d frames\n",
0668                skb_queue_len(&sock->tx_queue));
0669         return -ENOBUFS;
0670     }
0671
0672     msg_data = kmalloc(len, GFP_USER | __GFP_NOWARN);
0673     if (msg_data == NULL)
0674         return -ENOMEM;
0675
0676     if (memcpy_from_msg(msg_data, msg, len)) {
0677         kfree(msg_data);
0678         return -EFAULT;
0679     }
0680
0681     remaining_len = len;
0682     msg_ptr = msg_data;
0683
0684     do {
0685         remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
0686                 LLCP_DEFAULT_MIU : sock->remote_miu;
0687
0688         frag_len = min_t(size_t, remote_miu, remaining_len);
0689
0690         pr_debug("Fragment %zd bytes remaining %zd",
0691              frag_len, remaining_len);
0692
0693         pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
0694                     frag_len + LLCP_SEQUENCE_SIZE);
0695         if (pdu == NULL) {
0696             kfree(msg_data);
0697             return -ENOMEM;
0698         }
0699
0700         skb_put(pdu, LLCP_SEQUENCE_SIZE);
0701
0702         if (likely(frag_len > 0))
0703             skb_put_data(pdu, msg_ptr, frag_len);
0704
0705         skb_queue_tail(&sock->tx_queue, pdu);
0706
0707         lock_sock(sk);
0708
0709         nfc_llcp_queue_i_frames(sock);
0710
0711         release_sock(sk);
0712
0713         remaining_len -= frag_len;
0714         msg_ptr += frag_len;
0715     } while (remaining_len > 0);
0716
0717     kfree(msg_data);
0718
0719     return len;
0720 }
0721
0722 int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
0723                struct msghdr *msg, size_t len)
0724 {
0725     struct sk_buff *pdu;
0726     struct nfc_llcp_local *local;
0727     size_t frag_len = 0, remaining_len;
0728     u8 *msg_ptr, *msg_data;
0729     u16 remote_miu;
0730     int err;
0731
0732     pr_debug("Send UI frame len %zd\n", len);
0733
0734     local = sock->local;
0735     if (local == NULL)
0736         return -ENODEV;
0737
0738     msg_data = kmalloc(len, GFP_USER | __GFP_NOWARN);
0739     if (msg_data == NULL)
0740         return -ENOMEM;
0741
0742     if (memcpy_from_msg(msg_data, msg, len)) {
0743         kfree(msg_data);
0744         return -EFAULT;
0745     }
0746
0747     remaining_len = len;
0748     msg_ptr = msg_data;
0749
0750     do {
0751         remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
0752                 local->remote_miu : sock->remote_miu;
0753
0754         frag_len = min_t(size_t, remote_miu, remaining_len);
0755
0756         pr_debug("Fragment %zd bytes remaining %zd",
0757              frag_len, remaining_len);
0758
0759         pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, 0,
0760                      frag_len + LLCP_HEADER_SIZE, &err);
0761         if (pdu == NULL) {
0762             pr_err("Could not allocate PDU (error=%d)\n", err);
0763             len -= remaining_len;
0764             if (len == 0)
0765                 len = err;
0766             break;
0767         }
0768
0769         pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
0770
0771         if (likely(frag_len > 0))
0772             skb_put_data(pdu, msg_ptr, frag_len);
0773
0774         /* No need to check for the peer RW for UI frames */
0775         skb_queue_tail(&local->tx_queue, pdu);
0776
0777         remaining_len -= frag_len;
0778         msg_ptr += frag_len;
0779     } while (remaining_len > 0);
0780
0781     kfree(msg_data);
0782
0783     return len;
0784 }
0785
0786 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
0787 {
0788     struct sk_buff *skb;
0789     struct nfc_llcp_local *local;
0790
0791     pr_debug("Send rr nr %d\n", sock->recv_n);
0792
0793     local = sock->local;
0794     if (local == NULL)
0795         return -ENODEV;
0796
0797     skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
0798     if (skb == NULL)
0799         return -ENOMEM;
0800
0801     skb_put(skb, LLCP_SEQUENCE_SIZE);
0802
0803     skb->data[2] = sock->recv_n;
0804
0805     skb_queue_head(&local->tx_queue, skb);
0806
0807     return 0;
0808 }