/* Send NCI data */ int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb) { struct nci_conn_info *conn_info; int rc = 0; pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len); conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id); if (!conn_info) { rc = -EPROTO; goto free_exit; } /* check if the packet need to be fragmented */ if (skb->len <= conn_info->max_pkt_payload_len) { /* no need to fragment packet */ nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST); skb_queue_tail(&ndev->tx_q, skb); } else { /* fragment packet and queue the fragments */ rc = nci_queue_tx_data_frags(ndev, conn_id, skb); if (rc) { pr_err("failed to fragment tx data packet\n"); goto free_exit; } } ndev->cur_conn_id = conn_id; queue_work(ndev->tx_wq, &ndev->tx_work); goto exit; free_exit: kfree_skb(skb); exit: return rc; }
static int nci_queue_tx_data_frags(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb) { int total_len = skb->len; unsigned char *data = skb->data; unsigned long flags; struct sk_buff_head frags_q; struct sk_buff *skb_frag; int frag_len; int rc = 0; nfc_dbg("entry, conn_id 0x%x, total_len %d", conn_id, total_len); __skb_queue_head_init(&frags_q); while (total_len) { frag_len = min_t(int, total_len, ndev->max_data_pkt_payload_size); skb_frag = nci_skb_alloc(ndev, (NCI_DATA_HDR_SIZE + frag_len), GFP_KERNEL); if (skb_frag == NULL) { rc = -ENOMEM; goto free_exit; } skb_reserve(skb_frag, NCI_DATA_HDR_SIZE); /* first, copy the data */ memcpy(skb_put(skb_frag, frag_len), data, frag_len); /* second, set the header */ nci_push_data_hdr(ndev, conn_id, skb_frag, ((total_len == frag_len) ? (NCI_PBF_LAST) : (NCI_PBF_CONT))); __skb_queue_tail(&frags_q, skb_frag); data += frag_len; total_len -= frag_len; nfc_dbg("frag_len %d, remaining total_len %d", frag_len, total_len); } /* queue all fragments atomically */ spin_lock_irqsave(&ndev->tx_q.lock, flags); while ((skb_frag = __skb_dequeue(&frags_q)) != NULL) __skb_queue_tail(&ndev->tx_q, skb_frag); spin_unlock_irqrestore(&ndev->tx_q.lock, flags); /* free the original skb */ kfree_skb(skb); goto exit; free_exit: while ((skb_frag = __skb_dequeue(&frags_q)) != NULL) kfree_skb(skb_frag); exit: return rc; }