nfcsim.c 9.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * NFC hardware simulation driver
  4. * Copyright (c) 2013, Intel Corporation.
  5. */
  6. #include <linux/device.h>
  7. #include <linux/kernel.h>
  8. #include <linux/module.h>
  9. #include <linux/ctype.h>
  10. #include <linux/debugfs.h>
  11. #include <linux/nfc.h>
  12. #include <net/nfc/nfc.h>
  13. #include <net/nfc/digital.h>
  14. #define NFCSIM_ERR(d, fmt, args...) nfc_err(&d->nfc_digital_dev->nfc_dev->dev, \
  15. "%s: " fmt, __func__, ## args)
  16. #define NFCSIM_DBG(d, fmt, args...) dev_dbg(&d->nfc_digital_dev->nfc_dev->dev, \
  17. "%s: " fmt, __func__, ## args)
  18. #define NFCSIM_VERSION "0.2"
  19. #define NFCSIM_MODE_NONE 0
  20. #define NFCSIM_MODE_INITIATOR 1
  21. #define NFCSIM_MODE_TARGET 2
  22. #define NFCSIM_CAPABILITIES (NFC_DIGITAL_DRV_CAPS_IN_CRC | \
  23. NFC_DIGITAL_DRV_CAPS_TG_CRC)
  24. struct nfcsim {
  25. struct nfc_digital_dev *nfc_digital_dev;
  26. struct work_struct recv_work;
  27. struct delayed_work send_work;
  28. struct nfcsim_link *link_in;
  29. struct nfcsim_link *link_out;
  30. bool up;
  31. u8 mode;
  32. u8 rf_tech;
  33. u16 recv_timeout;
  34. nfc_digital_cmd_complete_t cb;
  35. void *arg;
  36. u8 dropframe;
  37. };
  38. struct nfcsim_link {
  39. struct mutex lock;
  40. u8 rf_tech;
  41. u8 mode;
  42. u8 shutdown;
  43. struct sk_buff *skb;
  44. wait_queue_head_t recv_wait;
  45. u8 cond;
  46. };
  47. static struct nfcsim_link *nfcsim_link_new(void)
  48. {
  49. struct nfcsim_link *link;
  50. link = kzalloc(sizeof(struct nfcsim_link), GFP_KERNEL);
  51. if (!link)
  52. return NULL;
  53. mutex_init(&link->lock);
  54. init_waitqueue_head(&link->recv_wait);
  55. return link;
  56. }
  57. static void nfcsim_link_free(struct nfcsim_link *link)
  58. {
  59. dev_kfree_skb(link->skb);
  60. kfree(link);
  61. }
  62. static void nfcsim_link_recv_wake(struct nfcsim_link *link)
  63. {
  64. link->cond = 1;
  65. wake_up_interruptible(&link->recv_wait);
  66. }
  67. static void nfcsim_link_set_skb(struct nfcsim_link *link, struct sk_buff *skb,
  68. u8 rf_tech, u8 mode)
  69. {
  70. mutex_lock(&link->lock);
  71. dev_kfree_skb(link->skb);
  72. link->skb = skb;
  73. link->rf_tech = rf_tech;
  74. link->mode = mode;
  75. mutex_unlock(&link->lock);
  76. }
  77. static void nfcsim_link_recv_cancel(struct nfcsim_link *link)
  78. {
  79. mutex_lock(&link->lock);
  80. link->mode = NFCSIM_MODE_NONE;
  81. mutex_unlock(&link->lock);
  82. nfcsim_link_recv_wake(link);
  83. }
  84. static void nfcsim_link_shutdown(struct nfcsim_link *link)
  85. {
  86. mutex_lock(&link->lock);
  87. link->shutdown = 1;
  88. link->mode = NFCSIM_MODE_NONE;
  89. mutex_unlock(&link->lock);
  90. nfcsim_link_recv_wake(link);
  91. }
  92. static struct sk_buff *nfcsim_link_recv_skb(struct nfcsim_link *link,
  93. int timeout, u8 rf_tech, u8 mode)
  94. {
  95. int rc;
  96. struct sk_buff *skb;
  97. rc = wait_event_interruptible_timeout(link->recv_wait,
  98. link->cond,
  99. msecs_to_jiffies(timeout));
  100. mutex_lock(&link->lock);
  101. skb = link->skb;
  102. link->skb = NULL;
  103. if (!rc) {
  104. rc = -ETIMEDOUT;
  105. goto done;
  106. }
  107. if (!skb || link->rf_tech != rf_tech || link->mode == mode) {
  108. rc = -EINVAL;
  109. goto done;
  110. }
  111. if (link->shutdown) {
  112. rc = -ENODEV;
  113. goto done;
  114. }
  115. done:
  116. mutex_unlock(&link->lock);
  117. if (rc < 0) {
  118. dev_kfree_skb(skb);
  119. skb = ERR_PTR(rc);
  120. }
  121. link->cond = 0;
  122. return skb;
  123. }
  124. static void nfcsim_send_wq(struct work_struct *work)
  125. {
  126. struct nfcsim *dev = container_of(work, struct nfcsim, send_work.work);
  127. /*
  128. * To effectively send data, the device just wake up its link_out which
  129. * is the link_in of the peer device. The exchanged skb has already been
  130. * stored in the dev->link_out through nfcsim_link_set_skb().
  131. */
  132. nfcsim_link_recv_wake(dev->link_out);
  133. }
  134. static void nfcsim_recv_wq(struct work_struct *work)
  135. {
  136. struct nfcsim *dev = container_of(work, struct nfcsim, recv_work);
  137. struct sk_buff *skb;
  138. skb = nfcsim_link_recv_skb(dev->link_in, dev->recv_timeout,
  139. dev->rf_tech, dev->mode);
  140. if (!dev->up) {
  141. NFCSIM_ERR(dev, "Device is down\n");
  142. if (!IS_ERR(skb))
  143. dev_kfree_skb(skb);
  144. return;
  145. }
  146. dev->cb(dev->nfc_digital_dev, dev->arg, skb);
  147. }
  148. static int nfcsim_send(struct nfc_digital_dev *ddev, struct sk_buff *skb,
  149. u16 timeout, nfc_digital_cmd_complete_t cb, void *arg)
  150. {
  151. struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
  152. u8 delay;
  153. if (!dev->up) {
  154. NFCSIM_ERR(dev, "Device is down\n");
  155. return -ENODEV;
  156. }
  157. dev->recv_timeout = timeout;
  158. dev->cb = cb;
  159. dev->arg = arg;
  160. schedule_work(&dev->recv_work);
  161. if (dev->dropframe) {
  162. NFCSIM_DBG(dev, "dropping frame (out of %d)\n", dev->dropframe);
  163. dev_kfree_skb(skb);
  164. dev->dropframe--;
  165. return 0;
  166. }
  167. if (skb) {
  168. nfcsim_link_set_skb(dev->link_out, skb, dev->rf_tech,
  169. dev->mode);
  170. /* Add random delay (between 3 and 10 ms) before sending data */
  171. get_random_bytes(&delay, 1);
  172. delay = 3 + (delay & 0x07);
  173. schedule_delayed_work(&dev->send_work, msecs_to_jiffies(delay));
  174. }
  175. return 0;
  176. }
  177. static void nfcsim_abort_cmd(struct nfc_digital_dev *ddev)
  178. {
  179. const struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
  180. nfcsim_link_recv_cancel(dev->link_in);
  181. }
  182. static int nfcsim_switch_rf(struct nfc_digital_dev *ddev, bool on)
  183. {
  184. struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
  185. dev->up = on;
  186. return 0;
  187. }
  188. static int nfcsim_in_configure_hw(struct nfc_digital_dev *ddev,
  189. int type, int param)
  190. {
  191. struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
  192. switch (type) {
  193. case NFC_DIGITAL_CONFIG_RF_TECH:
  194. dev->up = true;
  195. dev->mode = NFCSIM_MODE_INITIATOR;
  196. dev->rf_tech = param;
  197. break;
  198. case NFC_DIGITAL_CONFIG_FRAMING:
  199. break;
  200. default:
  201. NFCSIM_ERR(dev, "Invalid configuration type: %d\n", type);
  202. return -EINVAL;
  203. }
  204. return 0;
  205. }
  206. static int nfcsim_in_send_cmd(struct nfc_digital_dev *ddev,
  207. struct sk_buff *skb, u16 timeout,
  208. nfc_digital_cmd_complete_t cb, void *arg)
  209. {
  210. return nfcsim_send(ddev, skb, timeout, cb, arg);
  211. }
  212. static int nfcsim_tg_configure_hw(struct nfc_digital_dev *ddev,
  213. int type, int param)
  214. {
  215. struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
  216. switch (type) {
  217. case NFC_DIGITAL_CONFIG_RF_TECH:
  218. dev->up = true;
  219. dev->mode = NFCSIM_MODE_TARGET;
  220. dev->rf_tech = param;
  221. break;
  222. case NFC_DIGITAL_CONFIG_FRAMING:
  223. break;
  224. default:
  225. NFCSIM_ERR(dev, "Invalid configuration type: %d\n", type);
  226. return -EINVAL;
  227. }
  228. return 0;
  229. }
  230. static int nfcsim_tg_send_cmd(struct nfc_digital_dev *ddev,
  231. struct sk_buff *skb, u16 timeout,
  232. nfc_digital_cmd_complete_t cb, void *arg)
  233. {
  234. return nfcsim_send(ddev, skb, timeout, cb, arg);
  235. }
  236. static int nfcsim_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
  237. nfc_digital_cmd_complete_t cb, void *arg)
  238. {
  239. return nfcsim_send(ddev, NULL, timeout, cb, arg);
  240. }
  241. static const struct nfc_digital_ops nfcsim_digital_ops = {
  242. .in_configure_hw = nfcsim_in_configure_hw,
  243. .in_send_cmd = nfcsim_in_send_cmd,
  244. .tg_listen = nfcsim_tg_listen,
  245. .tg_configure_hw = nfcsim_tg_configure_hw,
  246. .tg_send_cmd = nfcsim_tg_send_cmd,
  247. .abort_cmd = nfcsim_abort_cmd,
  248. .switch_rf = nfcsim_switch_rf,
  249. };
  250. static struct dentry *nfcsim_debugfs_root;
  251. static void nfcsim_debugfs_init(void)
  252. {
  253. nfcsim_debugfs_root = debugfs_create_dir("nfcsim", NULL);
  254. }
  255. static void nfcsim_debugfs_remove(void)
  256. {
  257. debugfs_remove_recursive(nfcsim_debugfs_root);
  258. }
  259. static void nfcsim_debugfs_init_dev(struct nfcsim *dev)
  260. {
  261. struct dentry *dev_dir;
  262. char devname[5]; /* nfcX\0 */
  263. u32 idx;
  264. int n;
  265. if (!nfcsim_debugfs_root) {
  266. NFCSIM_ERR(dev, "nfcsim debugfs not initialized\n");
  267. return;
  268. }
  269. idx = dev->nfc_digital_dev->nfc_dev->idx;
  270. n = snprintf(devname, sizeof(devname), "nfc%d", idx);
  271. if (n >= sizeof(devname)) {
  272. NFCSIM_ERR(dev, "Could not compute dev name for dev %d\n", idx);
  273. return;
  274. }
  275. dev_dir = debugfs_create_dir(devname, nfcsim_debugfs_root);
  276. if (!dev_dir) {
  277. NFCSIM_ERR(dev, "Could not create debugfs entries for nfc%d\n",
  278. idx);
  279. return;
  280. }
  281. debugfs_create_u8("dropframe", 0664, dev_dir, &dev->dropframe);
  282. }
  283. static struct nfcsim *nfcsim_device_new(struct nfcsim_link *link_in,
  284. struct nfcsim_link *link_out)
  285. {
  286. struct nfcsim *dev;
  287. int rc;
  288. dev = kzalloc(sizeof(struct nfcsim), GFP_KERNEL);
  289. if (!dev)
  290. return ERR_PTR(-ENOMEM);
  291. INIT_DELAYED_WORK(&dev->send_work, nfcsim_send_wq);
  292. INIT_WORK(&dev->recv_work, nfcsim_recv_wq);
  293. dev->nfc_digital_dev =
  294. nfc_digital_allocate_device(&nfcsim_digital_ops,
  295. NFC_PROTO_NFC_DEP_MASK,
  296. NFCSIM_CAPABILITIES,
  297. 0, 0);
  298. if (!dev->nfc_digital_dev) {
  299. kfree(dev);
  300. return ERR_PTR(-ENOMEM);
  301. }
  302. nfc_digital_set_drvdata(dev->nfc_digital_dev, dev);
  303. dev->link_in = link_in;
  304. dev->link_out = link_out;
  305. rc = nfc_digital_register_device(dev->nfc_digital_dev);
  306. if (rc) {
  307. pr_err("Could not register digital device (%d)\n", rc);
  308. nfc_digital_free_device(dev->nfc_digital_dev);
  309. kfree(dev);
  310. return ERR_PTR(rc);
  311. }
  312. nfcsim_debugfs_init_dev(dev);
  313. return dev;
  314. }
  315. static void nfcsim_device_free(struct nfcsim *dev)
  316. {
  317. nfc_digital_unregister_device(dev->nfc_digital_dev);
  318. dev->up = false;
  319. nfcsim_link_shutdown(dev->link_in);
  320. cancel_delayed_work_sync(&dev->send_work);
  321. cancel_work_sync(&dev->recv_work);
  322. nfc_digital_free_device(dev->nfc_digital_dev);
  323. kfree(dev);
  324. }
  325. static struct nfcsim *dev0;
  326. static struct nfcsim *dev1;
  327. static int __init nfcsim_init(void)
  328. {
  329. struct nfcsim_link *link0, *link1;
  330. int rc;
  331. link0 = nfcsim_link_new();
  332. link1 = nfcsim_link_new();
  333. if (!link0 || !link1) {
  334. rc = -ENOMEM;
  335. goto exit_err;
  336. }
  337. nfcsim_debugfs_init();
  338. dev0 = nfcsim_device_new(link0, link1);
  339. if (IS_ERR(dev0)) {
  340. rc = PTR_ERR(dev0);
  341. goto exit_err;
  342. }
  343. dev1 = nfcsim_device_new(link1, link0);
  344. if (IS_ERR(dev1)) {
  345. nfcsim_device_free(dev0);
  346. rc = PTR_ERR(dev1);
  347. goto exit_err;
  348. }
  349. pr_info("nfcsim " NFCSIM_VERSION " initialized\n");
  350. return 0;
  351. exit_err:
  352. pr_err("Failed to initialize nfcsim driver (%d)\n", rc);
  353. if (link0)
  354. nfcsim_link_free(link0);
  355. if (link1)
  356. nfcsim_link_free(link1);
  357. return rc;
  358. }
  359. static void __exit nfcsim_exit(void)
  360. {
  361. struct nfcsim_link *link0, *link1;
  362. link0 = dev0->link_in;
  363. link1 = dev0->link_out;
  364. nfcsim_device_free(dev0);
  365. nfcsim_device_free(dev1);
  366. nfcsim_link_free(link0);
  367. nfcsim_link_free(link1);
  368. nfcsim_debugfs_remove();
  369. }
  370. module_init(nfcsim_init);
  371. module_exit(nfcsim_exit);
  372. MODULE_DESCRIPTION("NFCSim driver ver " NFCSIM_VERSION);
  373. MODULE_VERSION(NFCSIM_VERSION);
  374. MODULE_LICENSE("GPL");