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