aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--drivers/nfc/nfcsim.c612
1 files changed, 258 insertions, 354 deletions
diff --git a/drivers/nfc/nfcsim.c b/drivers/nfc/nfcsim.c
index 93aaca586858..40b4846509f4 100644
--- a/drivers/nfc/nfcsim.c
+++ b/drivers/nfc/nfcsim.c
@@ -18,523 +18,427 @@
18#include <linux/module.h> 18#include <linux/module.h>
19#include <linux/nfc.h> 19#include <linux/nfc.h>
20#include <net/nfc/nfc.h> 20#include <net/nfc/nfc.h>
21#include <net/nfc/digital.h>
21 22
22#define DEV_ERR(_dev, fmt, args...) nfc_err(&_dev->nfc_dev->dev, \ 23#define NFCSIM_ERR(d, fmt, args...) nfc_err(&d->nfc_digital_dev->nfc_dev->dev, \
23 "%s: " fmt, __func__, ## args) 24 "%s: " fmt, __func__, ## args)
24 25
25#define DEV_DBG(_dev, fmt, args...) dev_dbg(&_dev->nfc_dev->dev, \ 26#define NFCSIM_DBG(d, fmt, args...) dev_dbg(&d->nfc_digital_dev->nfc_dev->dev, \
26 "%s: " fmt, __func__, ## args) 27 "%s: " fmt, __func__, ## args)
27 28
28#define NFCSIM_VERSION "0.1" 29#define NFCSIM_VERSION "0.2"
29 30
30#define NFCSIM_POLL_NONE 0 31#define NFCSIM_MODE_NONE 0
31#define NFCSIM_POLL_INITIATOR 1 32#define NFCSIM_MODE_INITIATOR 1
32#define NFCSIM_POLL_TARGET 2 33#define NFCSIM_MODE_TARGET 2
33#define NFCSIM_POLL_DUAL (NFCSIM_POLL_INITIATOR | NFCSIM_POLL_TARGET)
34 34
35#define RX_DEFAULT_DELAY 5 35#define NFCSIM_CAPABILITIES (NFC_DIGITAL_DRV_CAPS_IN_CRC | \
36 NFC_DIGITAL_DRV_CAPS_TG_CRC)
36 37
37struct nfcsim { 38struct nfcsim {
38 struct nfc_dev *nfc_dev; 39 struct nfc_digital_dev *nfc_digital_dev;
39 40
40 struct mutex lock; 41 struct work_struct recv_work;
41 42 struct delayed_work send_work;
42 struct delayed_work recv_work;
43 43
44 struct sk_buff *clone_skb; 44 struct nfcsim_link *link_in;
45 struct nfcsim_link *link_out;
45 46
46 struct delayed_work poll_work; 47 bool up;
47 u8 polling_mode; 48 u8 mode;
48 u8 curr_polling_mode; 49 u8 rf_tech;
49 50
50 u8 shutting_down; 51 u16 recv_timeout;
51 52
52 u8 up; 53 nfc_digital_cmd_complete_t cb;
54 void *arg;
55};
53 56
54 u8 initiator; 57struct nfcsim_link {
58 struct mutex lock;
55 59
56 u32 rx_delay; 60 u8 rf_tech;
61 u8 mode;
57 62
58 data_exchange_cb_t cb; 63 u8 shutdown;
59 void *cb_context;
60 64
61 struct nfcsim *peer_dev; 65 struct sk_buff *skb;
66 wait_queue_head_t recv_wait;
67 u8 cond;
62}; 68};
63 69
64static struct nfcsim *dev0; 70static struct nfcsim_link *nfcsim_link_new(void)
65static struct nfcsim *dev1;
66
67static struct workqueue_struct *wq;
68
69static void nfcsim_cleanup_dev(struct nfcsim *dev, u8 shutdown)
70{ 71{
71 DEV_DBG(dev, "shutdown=%d\n", shutdown); 72 struct nfcsim_link *link;
72
73 mutex_lock(&dev->lock);
74 73
75 dev->polling_mode = NFCSIM_POLL_NONE; 74 link = kzalloc(sizeof(struct nfcsim_link), GFP_KERNEL);
76 dev->shutting_down = shutdown; 75 if (!link)
77 dev->cb = NULL; 76 return NULL;
78 dev_kfree_skb(dev->clone_skb);
79 dev->clone_skb = NULL;
80 77
81 mutex_unlock(&dev->lock); 78 mutex_init(&link->lock);
79 init_waitqueue_head(&link->recv_wait);
82 80
83 cancel_delayed_work_sync(&dev->poll_work); 81 return link;
84 cancel_delayed_work_sync(&dev->recv_work);
85} 82}
86 83
87static int nfcsim_target_found(struct nfcsim *dev) 84static void nfcsim_link_free(struct nfcsim_link *link)
88{ 85{
89 struct nfc_target nfc_tgt; 86 dev_kfree_skb(link->skb);
87 kfree(link);
88}
90 89
91 DEV_DBG(dev, "\n"); 90static void nfcsim_link_recv_wake(struct nfcsim_link *link)
91{
92 link->cond = 1;
93 wake_up_interruptible(&link->recv_wait);
94}
92 95
93 memset(&nfc_tgt, 0, sizeof(struct nfc_target)); 96static void nfcsim_link_set_skb(struct nfcsim_link *link, struct sk_buff *skb,
97 u8 rf_tech, u8 mode)
98{
99 mutex_lock(&link->lock);
94 100
95 nfc_tgt.supported_protocols = NFC_PROTO_NFC_DEP_MASK; 101 dev_kfree_skb(link->skb);
96 nfc_targets_found(dev->nfc_dev, &nfc_tgt, 1); 102 link->skb = skb;
103 link->rf_tech = rf_tech;
104 link->mode = mode;
97 105
98 return 0; 106 mutex_unlock(&link->lock);
99} 107}
100 108
101static int nfcsim_dev_up(struct nfc_dev *nfc_dev) 109static void nfcsim_link_recv_cancel(struct nfcsim_link *link)
102{ 110{
103 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 111 mutex_lock(&link->lock);
104 112
105 DEV_DBG(dev, "\n"); 113 link->mode = NFCSIM_MODE_NONE;
106 114
107 mutex_lock(&dev->lock); 115 mutex_unlock(&link->lock);
108 116
109 dev->up = 1; 117 nfcsim_link_recv_wake(link);
110
111 mutex_unlock(&dev->lock);
112
113 return 0;
114} 118}
115 119
116static int nfcsim_dev_down(struct nfc_dev *nfc_dev) 120static void nfcsim_link_shutdown(struct nfcsim_link *link)
117{ 121{
118 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 122 mutex_lock(&link->lock);
119
120 DEV_DBG(dev, "\n");
121 123
122 mutex_lock(&dev->lock); 124 link->shutdown = 1;
125 link->mode = NFCSIM_MODE_NONE;
123 126
124 dev->up = 0; 127 mutex_unlock(&link->lock);
125 128
126 mutex_unlock(&dev->lock); 129 nfcsim_link_recv_wake(link);
127
128 return 0;
129} 130}
130 131
131static int nfcsim_dep_link_up(struct nfc_dev *nfc_dev, 132static struct sk_buff *nfcsim_link_recv_skb(struct nfcsim_link *link,
132 struct nfc_target *target, 133 int timeout, u8 rf_tech, u8 mode)
133 u8 comm_mode, u8 *gb, size_t gb_len)
134{ 134{
135 int rc; 135 int rc;
136 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 136 struct sk_buff *skb;
137 struct nfcsim *peer = dev->peer_dev;
138 u8 *remote_gb;
139 size_t remote_gb_len;
140 137
141 DEV_DBG(dev, "target_idx: %d, comm_mode: %d\n", target->idx, comm_mode); 138 rc = wait_event_interruptible_timeout(link->recv_wait,
139 link->cond,
140 msecs_to_jiffies(timeout));
142 141
143 mutex_lock(&peer->lock); 142 mutex_lock(&link->lock);
144 143
145 nfc_tm_activated(peer->nfc_dev, NFC_PROTO_NFC_DEP_MASK, 144 skb = link->skb;
146 NFC_COMM_ACTIVE, gb, gb_len); 145 link->skb = NULL;
147 146
148 remote_gb = nfc_get_local_general_bytes(peer->nfc_dev, &remote_gb_len); 147 if (!rc) {
149 if (!remote_gb) { 148 rc = -ETIMEDOUT;
150 DEV_ERR(peer, "Can't get remote general bytes\n"); 149 goto done;
150 }
151 151
152 mutex_unlock(&peer->lock); 152 if (!skb || link->rf_tech != rf_tech || link->mode == mode) {
153 return -EINVAL; 153 rc = -EINVAL;
154 goto done;
154 } 155 }
155 156
156 mutex_unlock(&peer->lock); 157 if (link->shutdown) {
158 rc = -ENODEV;
159 goto done;
160 }
157 161
158 mutex_lock(&dev->lock); 162done:
163 mutex_unlock(&link->lock);
159 164
160 rc = nfc_set_remote_general_bytes(nfc_dev, remote_gb, remote_gb_len); 165 if (rc < 0) {
161 if (rc) { 166 dev_kfree_skb(skb);
162 DEV_ERR(dev, "Can't set remote general bytes\n"); 167 skb = ERR_PTR(rc);
163 mutex_unlock(&dev->lock);
164 return rc;
165 } 168 }
166 169
167 rc = nfc_dep_link_is_up(nfc_dev, target->idx, NFC_COMM_ACTIVE, 170 link->cond = 0;
168 NFC_RF_INITIATOR);
169
170 mutex_unlock(&dev->lock);
171 171
172 return rc; 172 return skb;
173} 173}
174 174
175static int nfcsim_dep_link_down(struct nfc_dev *nfc_dev) 175static void nfcsim_send_wq(struct work_struct *work)
176{ 176{
177 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 177 struct nfcsim *dev = container_of(work, struct nfcsim, send_work.work);
178 178
179 DEV_DBG(dev, "\n"); 179 /*
180 180 * To effectively send data, the device just wake up its link_out which
181 nfcsim_cleanup_dev(dev, 0); 181 * is the link_in of the peer device. The exchanged skb has already been
182 182 * stored in the dev->link_out through nfcsim_link_set_skb().
183 return 0; 183 */
184 nfcsim_link_recv_wake(dev->link_out);
184} 185}
185 186
186static int nfcsim_start_poll(struct nfc_dev *nfc_dev, 187static void nfcsim_recv_wq(struct work_struct *work)
187 u32 im_protocols, u32 tm_protocols)
188{ 188{
189 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 189 struct nfcsim *dev = container_of(work, struct nfcsim, recv_work);
190 int rc; 190 struct sk_buff *skb;
191
192 mutex_lock(&dev->lock);
193 191
194 if (dev->polling_mode != NFCSIM_POLL_NONE) { 192 skb = nfcsim_link_recv_skb(dev->link_in, dev->recv_timeout,
195 DEV_ERR(dev, "Already in polling mode\n"); 193 dev->rf_tech, dev->mode);
196 rc = -EBUSY;
197 goto exit;
198 }
199 194
200 if (im_protocols & NFC_PROTO_NFC_DEP_MASK) 195 if (!dev->up) {
201 dev->polling_mode |= NFCSIM_POLL_INITIATOR; 196 NFCSIM_ERR(dev, "Device is down\n");
202 197
203 if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) 198 if (!IS_ERR(skb))
204 dev->polling_mode |= NFCSIM_POLL_TARGET; 199 dev_kfree_skb(skb);
205 200
206 if (dev->polling_mode == NFCSIM_POLL_NONE) { 201 skb = ERR_PTR(-ENODEV);
207 DEV_ERR(dev, "Unsupported polling mode\n");
208 rc = -EINVAL;
209 goto exit;
210 } 202 }
211 203
212 dev->initiator = 0; 204 dev->cb(dev->nfc_digital_dev, dev->arg, skb);
213 dev->curr_polling_mode = NFCSIM_POLL_NONE;
214
215 queue_delayed_work(wq, &dev->poll_work, 0);
216
217 DEV_DBG(dev, "Start polling: im: 0x%X, tm: 0x%X\n", im_protocols,
218 tm_protocols);
219
220 rc = 0;
221exit:
222 mutex_unlock(&dev->lock);
223
224 return rc;
225} 205}
226 206
227static void nfcsim_stop_poll(struct nfc_dev *nfc_dev) 207static int nfcsim_send(struct nfc_digital_dev *ddev, struct sk_buff *skb,
208 u16 timeout, nfc_digital_cmd_complete_t cb, void *arg)
228{ 209{
229 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 210 struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
211 u8 delay;
230 212
231 DEV_DBG(dev, "Stop poll\n"); 213 if (!dev->up) {
214 NFCSIM_ERR(dev, "Device is down\n");
215 return -ENODEV;
216 }
217
218 dev->recv_timeout = timeout;
219 dev->cb = cb;
220 dev->arg = arg;
232 221
233 mutex_lock(&dev->lock); 222 schedule_work(&dev->recv_work);
234 223
235 dev->polling_mode = NFCSIM_POLL_NONE; 224 if (skb) {
225 nfcsim_link_set_skb(dev->link_out, skb, dev->rf_tech,
226 dev->mode);
236 227
237 mutex_unlock(&dev->lock); 228 /* Add random delay (between 3 and 10 ms) before sending data */
229 get_random_bytes(&delay, 1);
230 delay = 3 + (delay & 0x07);
238 231
239 cancel_delayed_work_sync(&dev->poll_work); 232 schedule_delayed_work(&dev->send_work, msecs_to_jiffies(delay));
233 }
234
235 return 0;
240} 236}
241 237
242static int nfcsim_activate_target(struct nfc_dev *nfc_dev, 238static void nfcsim_abort_cmd(struct nfc_digital_dev *ddev)
243 struct nfc_target *target, u32 protocol)
244{ 239{
245 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 240 struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
246
247 DEV_DBG(dev, "\n");
248 241
249 return -ENOTSUPP; 242 nfcsim_link_recv_cancel(dev->link_in);
250} 243}
251 244
252static void nfcsim_deactivate_target(struct nfc_dev *nfc_dev, 245static int nfcsim_switch_rf(struct nfc_digital_dev *ddev, bool on)
253 struct nfc_target *target, u8 mode)
254{ 246{
255 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 247 struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
256 248
257 DEV_DBG(dev, "\n"); 249 dev->up = on;
250
251 return 0;
258} 252}
259 253
260static void nfcsim_wq_recv(struct work_struct *work) 254static int nfcsim_in_configure_hw(struct nfc_digital_dev *ddev,
255 int type, int param)
261{ 256{
262 struct nfcsim *dev = container_of(work, struct nfcsim, 257 struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
263 recv_work.work);
264 258
265 mutex_lock(&dev->lock); 259 switch (type) {
260 case NFC_DIGITAL_CONFIG_RF_TECH:
261 dev->up = true;
262 dev->mode = NFCSIM_MODE_INITIATOR;
263 dev->rf_tech = param;
264 break;
266 265
267 if (dev->shutting_down || !dev->up || !dev->clone_skb) { 266 case NFC_DIGITAL_CONFIG_FRAMING:
268 dev_kfree_skb(dev->clone_skb); 267 break;
269 goto exit;
270 }
271 268
272 if (dev->initiator) { 269 default:
273 if (!dev->cb) { 270 NFCSIM_ERR(dev, "Invalid configuration type: %d\n", type);
274 DEV_ERR(dev, "Null recv callback\n"); 271 return -EINVAL;
275 dev_kfree_skb(dev->clone_skb);
276 goto exit;
277 }
278
279 dev->cb(dev->cb_context, dev->clone_skb, 0);
280 dev->cb = NULL;
281 } else {
282 nfc_tm_data_received(dev->nfc_dev, dev->clone_skb);
283 } 272 }
284 273
285exit: 274 return 0;
286 dev->clone_skb = NULL;
287
288 mutex_unlock(&dev->lock);
289} 275}
290 276
291static int nfcsim_tx(struct nfc_dev *nfc_dev, struct nfc_target *target, 277static int nfcsim_in_send_cmd(struct nfc_digital_dev *ddev,
292 struct sk_buff *skb, data_exchange_cb_t cb, 278 struct sk_buff *skb, u16 timeout,
293 void *cb_context) 279 nfc_digital_cmd_complete_t cb, void *arg)
294{ 280{
295 struct nfcsim *dev = nfc_get_drvdata(nfc_dev); 281 return nfcsim_send(ddev, skb, timeout, cb, arg);
296 struct nfcsim *peer = dev->peer_dev; 282}
297 int err;
298
299 mutex_lock(&dev->lock);
300
301 if (dev->shutting_down || !dev->up) {
302 mutex_unlock(&dev->lock);
303 err = -ENODEV;
304 goto exit;
305 }
306
307 dev->cb = cb;
308 dev->cb_context = cb_context;
309 283
310 mutex_unlock(&dev->lock); 284static int nfcsim_tg_configure_hw(struct nfc_digital_dev *ddev,
285 int type, int param)
286{
287 struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
311 288
312 mutex_lock(&peer->lock); 289 switch (type) {
290 case NFC_DIGITAL_CONFIG_RF_TECH:
291 dev->up = true;
292 dev->mode = NFCSIM_MODE_TARGET;
293 dev->rf_tech = param;
294 break;
313 295
314 peer->clone_skb = skb_clone(skb, GFP_KERNEL); 296 case NFC_DIGITAL_CONFIG_FRAMING:
297 break;
315 298
316 if (!peer->clone_skb) { 299 default:
317 DEV_ERR(dev, "skb_clone failed\n"); 300 NFCSIM_ERR(dev, "Invalid configuration type: %d\n", type);
318 mutex_unlock(&peer->lock); 301 return -EINVAL;
319 err = -ENOMEM;
320 goto exit;
321 } 302 }
322 303
323 /* This simulates an arbitrary transmission delay between the 2 devices. 304 return 0;
324 * If packet transmission occurs immediately between them, we have a
325 * non-stop flow of several tens of thousands SYMM packets per second
326 * and a burning cpu.
327 */
328 queue_delayed_work(wq, &peer->recv_work,
329 msecs_to_jiffies(dev->rx_delay));
330
331 mutex_unlock(&peer->lock);
332
333 err = 0;
334exit:
335 dev_kfree_skb(skb);
336
337 return err;
338} 305}
339 306
340static int nfcsim_im_transceive(struct nfc_dev *nfc_dev, 307static int nfcsim_tg_send_cmd(struct nfc_digital_dev *ddev,
341 struct nfc_target *target, struct sk_buff *skb, 308 struct sk_buff *skb, u16 timeout,
342 data_exchange_cb_t cb, void *cb_context) 309 nfc_digital_cmd_complete_t cb, void *arg)
343{ 310{
344 return nfcsim_tx(nfc_dev, target, skb, cb, cb_context); 311 return nfcsim_send(ddev, skb, timeout, cb, arg);
345} 312}
346 313
347static int nfcsim_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb) 314static int nfcsim_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
315 nfc_digital_cmd_complete_t cb, void *arg)
348{ 316{
349 return nfcsim_tx(nfc_dev, NULL, skb, NULL, NULL); 317 return nfcsim_send(ddev, NULL, timeout, cb, arg);
350} 318}
351 319
352static struct nfc_ops nfcsim_nfc_ops = { 320static struct nfc_digital_ops nfcsim_digital_ops = {
353 .dev_up = nfcsim_dev_up, 321 .in_configure_hw = nfcsim_in_configure_hw,
354 .dev_down = nfcsim_dev_down, 322 .in_send_cmd = nfcsim_in_send_cmd,
355 .dep_link_up = nfcsim_dep_link_up,
356 .dep_link_down = nfcsim_dep_link_down,
357 .start_poll = nfcsim_start_poll,
358 .stop_poll = nfcsim_stop_poll,
359 .activate_target = nfcsim_activate_target,
360 .deactivate_target = nfcsim_deactivate_target,
361 .im_transceive = nfcsim_im_transceive,
362 .tm_send = nfcsim_tm_send,
363};
364
365static void nfcsim_set_polling_mode(struct nfcsim *dev)
366{
367 if (dev->polling_mode == NFCSIM_POLL_NONE) {
368 dev->curr_polling_mode = NFCSIM_POLL_NONE;
369 return;
370 }
371 323
372 if (dev->curr_polling_mode == NFCSIM_POLL_NONE) { 324 .tg_listen = nfcsim_tg_listen,
373 if (dev->polling_mode & NFCSIM_POLL_INITIATOR) 325 .tg_configure_hw = nfcsim_tg_configure_hw,
374 dev->curr_polling_mode = NFCSIM_POLL_INITIATOR; 326 .tg_send_cmd = nfcsim_tg_send_cmd,
375 else
376 dev->curr_polling_mode = NFCSIM_POLL_TARGET;
377 327
378 return; 328 .abort_cmd = nfcsim_abort_cmd,
379 } 329 .switch_rf = nfcsim_switch_rf,
380 330};
381 if (dev->polling_mode == NFCSIM_POLL_DUAL) {
382 if (dev->curr_polling_mode == NFCSIM_POLL_TARGET)
383 dev->curr_polling_mode = NFCSIM_POLL_INITIATOR;
384 else
385 dev->curr_polling_mode = NFCSIM_POLL_TARGET;
386 }
387}
388 331
389static void nfcsim_wq_poll(struct work_struct *work) 332static struct nfcsim *nfcsim_device_new(struct nfcsim_link *link_in,
333 struct nfcsim_link *link_out)
390{ 334{
391 struct nfcsim *dev = container_of(work, struct nfcsim, poll_work.work); 335 struct nfcsim *dev;
392 struct nfcsim *peer = dev->peer_dev; 336 int rc;
393 337
394 /* These work items run on an ordered workqueue and are therefore 338 dev = kzalloc(sizeof(struct nfcsim), GFP_KERNEL);
395 * serialized. So we can take both mutexes without being dead locked. 339 if (!dev)
396 */ 340 return ERR_PTR(-ENOMEM);
397 mutex_lock(&dev->lock);
398 mutex_lock(&peer->lock);
399 341
400 nfcsim_set_polling_mode(dev); 342 INIT_DELAYED_WORK(&dev->send_work, nfcsim_send_wq);
343 INIT_WORK(&dev->recv_work, nfcsim_recv_wq);
401 344
402 if (dev->curr_polling_mode == NFCSIM_POLL_NONE) { 345 dev->nfc_digital_dev =
403 DEV_DBG(dev, "Not polling\n"); 346 nfc_digital_allocate_device(&nfcsim_digital_ops,
404 goto unlock; 347 NFC_PROTO_NFC_DEP_MASK,
348 NFCSIM_CAPABILITIES,
349 0, 0);
350 if (!dev->nfc_digital_dev) {
351 kfree(dev);
352 return ERR_PTR(-ENOMEM);
405 } 353 }
406 354
407 DEV_DBG(dev, "Polling as %s", 355 nfc_digital_set_drvdata(dev->nfc_digital_dev, dev);
408 dev->curr_polling_mode == NFCSIM_POLL_INITIATOR ?
409 "initiator\n" : "target\n");
410
411 if (dev->curr_polling_mode == NFCSIM_POLL_TARGET)
412 goto sched_work;
413 356
414 if (peer->curr_polling_mode == NFCSIM_POLL_TARGET) { 357 dev->link_in = link_in;
415 peer->polling_mode = NFCSIM_POLL_NONE; 358 dev->link_out = link_out;
416 dev->polling_mode = NFCSIM_POLL_NONE;
417 359
418 dev->initiator = 1; 360 rc = nfc_digital_register_device(dev->nfc_digital_dev);
419 361 if (rc) {
420 nfcsim_target_found(dev); 362 pr_err("Could not register digital device (%d)\n", rc);
363 nfc_digital_free_device(dev->nfc_digital_dev);
364 kfree(dev);
421 365
422 goto unlock; 366 return ERR_PTR(rc);
423 } 367 }
424 368
425sched_work: 369 return dev;
426 /* This defines the delay for an initiator to check if the other device
427 * is polling in target mode.
428 * If the device starts in dual mode polling, it switches between
429 * initiator and target at every round.
430 * Because the wq is ordered and only 1 work item is executed at a time,
431 * we'll always have one device polling as initiator and the other as
432 * target at some point, even if both are started in dual mode.
433 */
434 queue_delayed_work(wq, &dev->poll_work, msecs_to_jiffies(200));
435
436unlock:
437 mutex_unlock(&peer->lock);
438 mutex_unlock(&dev->lock);
439} 370}
440 371
441static struct nfcsim *nfcsim_init_dev(void) 372static void nfcsim_device_free(struct nfcsim *dev)
442{ 373{
443 struct nfcsim *dev; 374 nfc_digital_unregister_device(dev->nfc_digital_dev);
444 int rc = -ENOMEM;
445
446 dev = kzalloc(sizeof(*dev), GFP_KERNEL);
447 if (dev == NULL)
448 return ERR_PTR(-ENOMEM);
449
450 mutex_init(&dev->lock);
451
452 INIT_DELAYED_WORK(&dev->recv_work, nfcsim_wq_recv);
453 INIT_DELAYED_WORK(&dev->poll_work, nfcsim_wq_poll);
454
455 dev->nfc_dev = nfc_allocate_device(&nfcsim_nfc_ops,
456 NFC_PROTO_NFC_DEP_MASK,
457 0, 0);
458 if (!dev->nfc_dev)
459 goto error;
460 375
461 nfc_set_drvdata(dev->nfc_dev, dev); 376 dev->up = false;
462 377
463 rc = nfc_register_device(dev->nfc_dev); 378 nfcsim_link_shutdown(dev->link_in);
464 if (rc)
465 goto free_nfc_dev;
466 379
467 dev->rx_delay = RX_DEFAULT_DELAY; 380 cancel_delayed_work_sync(&dev->send_work);
468 return dev; 381 cancel_work_sync(&dev->recv_work);
469 382
470free_nfc_dev: 383 nfc_digital_free_device(dev->nfc_digital_dev);
471 nfc_free_device(dev->nfc_dev);
472 384
473error:
474 kfree(dev); 385 kfree(dev);
475
476 return ERR_PTR(rc);
477} 386}
478 387
479static void nfcsim_free_device(struct nfcsim *dev) 388static struct nfcsim *dev0;
480{ 389static struct nfcsim *dev1;
481 nfc_unregister_device(dev->nfc_dev);
482
483 nfc_free_device(dev->nfc_dev);
484
485 kfree(dev);
486}
487 390
488static int __init nfcsim_init(void) 391static int __init nfcsim_init(void)
489{ 392{
393 struct nfcsim_link *link0, *link1;
490 int rc; 394 int rc;
491 395
492 /* We need an ordered wq to ensure that poll_work items are executed 396 link0 = nfcsim_link_new();
493 * one at a time. 397 link1 = nfcsim_link_new();
494 */ 398 if (!link0 || !link1) {
495 wq = alloc_ordered_workqueue("nfcsim", 0);
496 if (!wq) {
497 rc = -ENOMEM; 399 rc = -ENOMEM;
498 goto exit; 400 goto exit_err;
499 } 401 }
500 402
501 dev0 = nfcsim_init_dev(); 403 dev0 = nfcsim_device_new(link0, link1);
502 if (IS_ERR(dev0)) { 404 if (IS_ERR(dev0)) {
503 rc = PTR_ERR(dev0); 405 rc = PTR_ERR(dev0);
504 goto exit; 406 goto exit_err;
505 } 407 }
506 408
507 dev1 = nfcsim_init_dev(); 409 dev1 = nfcsim_device_new(link1, link0);
508 if (IS_ERR(dev1)) { 410 if (IS_ERR(dev1)) {
509 kfree(dev0); 411 nfcsim_device_free(dev0);
510 412
511 rc = PTR_ERR(dev1); 413 rc = PTR_ERR(dev1);
512 goto exit; 414 goto exit_err;
513 } 415 }
514 416
515 dev0->peer_dev = dev1; 417 pr_info("nfcsim " NFCSIM_VERSION " initialized\n");
516 dev1->peer_dev = dev0;
517 418
518 pr_debug("NFCsim " NFCSIM_VERSION " initialized\n"); 419 return 0;
420
421exit_err:
422 pr_err("Failed to initialize nfcsim driver (%d)\n", rc);
519 423
520 rc = 0; 424 nfcsim_link_free(link0);
521exit: 425 nfcsim_link_free(link1);
522 if (rc)
523 pr_err("Failed to initialize nfcsim driver (%d)\n",
524 rc);
525 426
526 return rc; 427 return rc;
527} 428}
528 429
529static void __exit nfcsim_exit(void) 430static void __exit nfcsim_exit(void)
530{ 431{
531 nfcsim_cleanup_dev(dev0, 1); 432 struct nfcsim_link *link0, *link1;
532 nfcsim_cleanup_dev(dev1, 1); 433
434 link0 = dev0->link_in;
435 link1 = dev0->link_out;
533 436
534 nfcsim_free_device(dev0); 437 nfcsim_device_free(dev0);
535 nfcsim_free_device(dev1); 438 nfcsim_device_free(dev1);
536 439
537 destroy_workqueue(wq); 440 nfcsim_link_free(link0);
441 nfcsim_link_free(link1);
538} 442}
539 443
540module_init(nfcsim_init); 444module_init(nfcsim_init);
generated by cgit v1.2.2 (git 2.25.0) at 2025-04-25 13:11:17 -0400