nfcsim.c 10 KB

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