data.c 7.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * The NFC Controller Interface is the communication protocol between an
  4. * NFC Controller (NFCC) and a Device Host (DH).
  5. *
  6. * Copyright (C) 2011 Texas Instruments, Inc.
  7. * Copyright (C) 2014 Marvell International Ltd.
  8. *
  9. * Written by Ilan Elias <ilane@ti.com>
  10. */
  11. #define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
  12. #include <linux/types.h>
  13. #include <linux/interrupt.h>
  14. #include <linux/wait.h>
  15. #include <linux/bitops.h>
  16. #include <linux/skbuff.h>
  17. #include "../nfc.h"
  18. #include <net/nfc/nci.h>
  19. #include <net/nfc/nci_core.h>
  20. #include <linux/nfc.h>
  21. /* Complete data exchange transaction and forward skb to nfc core */
  22. void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
  23. __u8 conn_id, int err)
  24. {
  25. struct nci_conn_info *conn_info;
  26. data_exchange_cb_t cb;
  27. void *cb_context;
  28. conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
  29. if (!conn_info) {
  30. kfree_skb(skb);
  31. goto exit;
  32. }
  33. cb = conn_info->data_exchange_cb;
  34. cb_context = conn_info->data_exchange_cb_context;
  35. pr_debug("len %d, err %d\n", skb ? skb->len : 0, err);
  36. /* data exchange is complete, stop the data timer */
  37. del_timer_sync(&ndev->data_timer);
  38. clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
  39. if (cb) {
  40. /* forward skb to nfc core */
  41. cb(cb_context, skb, err);
  42. } else if (skb) {
  43. pr_err("no rx callback, dropping rx data...\n");
  44. /* no waiting callback, free skb */
  45. kfree_skb(skb);
  46. }
  47. exit:
  48. clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
  49. }
  50. /* ----------------- NCI TX Data ----------------- */
  51. static inline void nci_push_data_hdr(struct nci_dev *ndev,
  52. __u8 conn_id,
  53. struct sk_buff *skb,
  54. __u8 pbf)
  55. {
  56. struct nci_data_hdr *hdr;
  57. int plen = skb->len;
  58. hdr = skb_push(skb, NCI_DATA_HDR_SIZE);
  59. hdr->conn_id = conn_id;
  60. hdr->rfu = 0;
  61. hdr->plen = plen;
  62. nci_mt_set((__u8 *)hdr, NCI_MT_DATA_PKT);
  63. nci_pbf_set((__u8 *)hdr, pbf);
  64. }
  65. int nci_conn_max_data_pkt_payload_size(struct nci_dev *ndev, __u8 conn_id)
  66. {
  67. struct nci_conn_info *conn_info;
  68. conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
  69. if (!conn_info)
  70. return -EPROTO;
  71. return conn_info->max_pkt_payload_len;
  72. }
  73. EXPORT_SYMBOL(nci_conn_max_data_pkt_payload_size);
  74. static int nci_queue_tx_data_frags(struct nci_dev *ndev,
  75. __u8 conn_id,
  76. struct sk_buff *skb) {
  77. struct nci_conn_info *conn_info;
  78. int total_len = skb->len;
  79. unsigned char *data = skb->data;
  80. unsigned long flags;
  81. struct sk_buff_head frags_q;
  82. struct sk_buff *skb_frag;
  83. int frag_len;
  84. int rc = 0;
  85. pr_debug("conn_id 0x%x, total_len %d\n", conn_id, total_len);
  86. conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
  87. if (!conn_info) {
  88. rc = -EPROTO;
  89. goto exit;
  90. }
  91. __skb_queue_head_init(&frags_q);
  92. while (total_len) {
  93. frag_len =
  94. min_t(int, total_len, conn_info->max_pkt_payload_len);
  95. skb_frag = nci_skb_alloc(ndev,
  96. (NCI_DATA_HDR_SIZE + frag_len),
  97. GFP_KERNEL);
  98. if (skb_frag == NULL) {
  99. rc = -ENOMEM;
  100. goto free_exit;
  101. }
  102. skb_reserve(skb_frag, NCI_DATA_HDR_SIZE);
  103. /* first, copy the data */
  104. skb_put_data(skb_frag, data, frag_len);
  105. /* second, set the header */
  106. nci_push_data_hdr(ndev, conn_id, skb_frag,
  107. ((total_len == frag_len) ?
  108. (NCI_PBF_LAST) : (NCI_PBF_CONT)));
  109. __skb_queue_tail(&frags_q, skb_frag);
  110. data += frag_len;
  111. total_len -= frag_len;
  112. pr_debug("frag_len %d, remaining total_len %d\n",
  113. frag_len, total_len);
  114. }
  115. /* queue all fragments atomically */
  116. spin_lock_irqsave(&ndev->tx_q.lock, flags);
  117. while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
  118. __skb_queue_tail(&ndev->tx_q, skb_frag);
  119. spin_unlock_irqrestore(&ndev->tx_q.lock, flags);
  120. /* free the original skb */
  121. kfree_skb(skb);
  122. goto exit;
  123. free_exit:
  124. while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
  125. kfree_skb(skb_frag);
  126. exit:
  127. return rc;
  128. }
  129. /* Send NCI data */
  130. int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
  131. {
  132. struct nci_conn_info *conn_info;
  133. int rc = 0;
  134. pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len);
  135. conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
  136. if (!conn_info) {
  137. rc = -EPROTO;
  138. goto free_exit;
  139. }
  140. /* check if the packet need to be fragmented */
  141. if (skb->len <= conn_info->max_pkt_payload_len) {
  142. /* no need to fragment packet */
  143. nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST);
  144. skb_queue_tail(&ndev->tx_q, skb);
  145. } else {
  146. /* fragment packet and queue the fragments */
  147. rc = nci_queue_tx_data_frags(ndev, conn_id, skb);
  148. if (rc) {
  149. pr_err("failed to fragment tx data packet\n");
  150. goto free_exit;
  151. }
  152. }
  153. ndev->cur_conn_id = conn_id;
  154. queue_work(ndev->tx_wq, &ndev->tx_work);
  155. goto exit;
  156. free_exit:
  157. kfree_skb(skb);
  158. exit:
  159. return rc;
  160. }
  161. EXPORT_SYMBOL(nci_send_data);
  162. /* ----------------- NCI RX Data ----------------- */
  163. static void nci_add_rx_data_frag(struct nci_dev *ndev,
  164. struct sk_buff *skb,
  165. __u8 pbf, __u8 conn_id, __u8 status)
  166. {
  167. int reassembly_len;
  168. int err = 0;
  169. if (status) {
  170. err = status;
  171. goto exit;
  172. }
  173. if (ndev->rx_data_reassembly) {
  174. reassembly_len = ndev->rx_data_reassembly->len;
  175. /* first, make enough room for the already accumulated data */
  176. if (skb_cow_head(skb, reassembly_len)) {
  177. pr_err("error adding room for accumulated rx data\n");
  178. kfree_skb(skb);
  179. skb = NULL;
  180. kfree_skb(ndev->rx_data_reassembly);
  181. ndev->rx_data_reassembly = NULL;
  182. err = -ENOMEM;
  183. goto exit;
  184. }
  185. /* second, combine the two fragments */
  186. memcpy(skb_push(skb, reassembly_len),
  187. ndev->rx_data_reassembly->data,
  188. reassembly_len);
  189. /* third, free old reassembly */
  190. kfree_skb(ndev->rx_data_reassembly);
  191. ndev->rx_data_reassembly = NULL;
  192. }
  193. if (pbf == NCI_PBF_CONT) {
  194. /* need to wait for next fragment, store skb and exit */
  195. ndev->rx_data_reassembly = skb;
  196. return;
  197. }
  198. exit:
  199. if (ndev->nfc_dev->rf_mode == NFC_RF_TARGET) {
  200. /* Data received in Target mode, forward to nfc core */
  201. err = nfc_tm_data_received(ndev->nfc_dev, skb);
  202. if (err)
  203. pr_err("unable to handle received data\n");
  204. } else {
  205. nci_data_exchange_complete(ndev, skb, conn_id, err);
  206. }
  207. }
  208. /* Rx Data packet */
  209. void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
  210. {
  211. __u8 pbf = nci_pbf(skb->data);
  212. __u8 status = 0;
  213. __u8 conn_id = nci_conn_id(skb->data);
  214. struct nci_conn_info *conn_info;
  215. pr_debug("len %d\n", skb->len);
  216. pr_debug("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
  217. nci_pbf(skb->data),
  218. nci_conn_id(skb->data),
  219. nci_plen(skb->data));
  220. conn_info = nci_get_conn_info_by_conn_id(ndev, nci_conn_id(skb->data));
  221. if (!conn_info)
  222. return;
  223. /* strip the nci data header */
  224. skb_pull(skb, NCI_DATA_HDR_SIZE);
  225. if (ndev->target_active_prot == NFC_PROTO_MIFARE ||
  226. ndev->target_active_prot == NFC_PROTO_JEWEL ||
  227. ndev->target_active_prot == NFC_PROTO_FELICA ||
  228. ndev->target_active_prot == NFC_PROTO_ISO15693) {
  229. /* frame I/F => remove the status byte */
  230. pr_debug("frame I/F => remove the status byte\n");
  231. status = skb->data[skb->len - 1];
  232. skb_trim(skb, (skb->len - 1));
  233. }
  234. nci_add_rx_data_frag(ndev, skb, pbf, conn_id, nci_to_errno(status));
  235. }