nfcsim.c 10.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
/*
 * NFC hardware simulation driver
 * Copyright (c) 2013, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 */

#include <linux/device.h>
#include <linux/kernel.h>
#include <linux/module.h>
19 20
#include <linux/ctype.h>
#include <linux/debugfs.h>
21 22
#include <linux/nfc.h>
#include <net/nfc/nfc.h>
23
#include <net/nfc/digital.h>
24

25 26
#define NFCSIM_ERR(d, fmt, args...) nfc_err(&d->nfc_digital_dev->nfc_dev->dev, \
					    "%s: " fmt, __func__, ## args)
27

28 29
#define NFCSIM_DBG(d, fmt, args...) dev_dbg(&d->nfc_digital_dev->nfc_dev->dev, \
					    "%s: " fmt, __func__, ## args)
30

31
#define NFCSIM_VERSION "0.2"
32

33 34 35
#define NFCSIM_MODE_NONE	0
#define NFCSIM_MODE_INITIATOR	1
#define NFCSIM_MODE_TARGET	2
36

37 38
#define NFCSIM_CAPABILITIES (NFC_DIGITAL_DRV_CAPS_IN_CRC   | \
			     NFC_DIGITAL_DRV_CAPS_TG_CRC)
39

40
struct nfcsim {
41
	struct nfc_digital_dev *nfc_digital_dev;
42

43 44
	struct work_struct recv_work;
	struct delayed_work send_work;
45

46 47
	struct nfcsim_link *link_in;
	struct nfcsim_link *link_out;
48

49 50 51
	bool up;
	u8 mode;
	u8 rf_tech;
52

53
	u16 recv_timeout;
54

55 56
	nfc_digital_cmd_complete_t cb;
	void *arg;
57 58

	u8 dropframe;
59
};
60

61 62
struct nfcsim_link {
	struct mutex lock;
63

64 65
	u8 rf_tech;
	u8 mode;
66

67
	u8 shutdown;
68

69 70 71
	struct sk_buff *skb;
	wait_queue_head_t recv_wait;
	u8 cond;
72 73
};

74
static struct nfcsim_link *nfcsim_link_new(void)
75
{
76
	struct nfcsim_link *link;
77

78 79 80
	link = kzalloc(sizeof(struct nfcsim_link), GFP_KERNEL);
	if (!link)
		return NULL;
81

82 83
	mutex_init(&link->lock);
	init_waitqueue_head(&link->recv_wait);
84

85
	return link;
86 87
}

88
static void nfcsim_link_free(struct nfcsim_link *link)
89
{
90 91 92
	dev_kfree_skb(link->skb);
	kfree(link);
}
93

94 95 96 97 98
static void nfcsim_link_recv_wake(struct nfcsim_link *link)
{
	link->cond = 1;
	wake_up_interruptible(&link->recv_wait);
}
99

100 101 102 103
static void nfcsim_link_set_skb(struct nfcsim_link *link, struct sk_buff *skb,
				u8 rf_tech, u8 mode)
{
	mutex_lock(&link->lock);
104

105 106 107 108
	dev_kfree_skb(link->skb);
	link->skb = skb;
	link->rf_tech = rf_tech;
	link->mode = mode;
109

110
	mutex_unlock(&link->lock);
111 112
}

113
static void nfcsim_link_recv_cancel(struct nfcsim_link *link)
114
{
115
	mutex_lock(&link->lock);
116

117
	link->mode = NFCSIM_MODE_NONE;
118

119
	mutex_unlock(&link->lock);
120

121
	nfcsim_link_recv_wake(link);
122 123
}

124
static void nfcsim_link_shutdown(struct nfcsim_link *link)
125
{
126
	mutex_lock(&link->lock);
127

128 129
	link->shutdown = 1;
	link->mode = NFCSIM_MODE_NONE;
130

131
	mutex_unlock(&link->lock);
132

133
	nfcsim_link_recv_wake(link);
134 135
}

136 137
static struct sk_buff *nfcsim_link_recv_skb(struct nfcsim_link *link,
					    int timeout, u8 rf_tech, u8 mode)
138 139
{
	int rc;
140
	struct sk_buff *skb;
141

142 143 144
	rc = wait_event_interruptible_timeout(link->recv_wait,
					      link->cond,
					      msecs_to_jiffies(timeout));
145

146
	mutex_lock(&link->lock);
147

148 149
	skb = link->skb;
	link->skb = NULL;
150

151 152 153 154
	if (!rc) {
		rc = -ETIMEDOUT;
		goto done;
	}
155

156 157 158
	if (!skb || link->rf_tech != rf_tech || link->mode == mode) {
		rc = -EINVAL;
		goto done;
159 160
	}

161 162 163 164
	if (link->shutdown) {
		rc = -ENODEV;
		goto done;
	}
165

166 167
done:
	mutex_unlock(&link->lock);
168

169 170 171
	if (rc < 0) {
		dev_kfree_skb(skb);
		skb = ERR_PTR(rc);
172 173
	}

174
	link->cond = 0;
175

176
	return skb;
177 178
}

179
static void nfcsim_send_wq(struct work_struct *work)
180
{
181
	struct nfcsim *dev = container_of(work, struct nfcsim, send_work.work);
182

183 184 185 186 187 188
	/*
	 * To effectively send data, the device just wake up its link_out which
	 * is the link_in of the peer device. The exchanged skb has already been
	 * stored in the dev->link_out through nfcsim_link_set_skb().
	 */
	nfcsim_link_recv_wake(dev->link_out);
189 190
}

191
static void nfcsim_recv_wq(struct work_struct *work)
192
{
193 194
	struct nfcsim *dev = container_of(work, struct nfcsim, recv_work);
	struct sk_buff *skb;
195

196 197
	skb = nfcsim_link_recv_skb(dev->link_in, dev->recv_timeout,
				   dev->rf_tech, dev->mode);
198

199 200
	if (!dev->up) {
		NFCSIM_ERR(dev, "Device is down\n");
201

202 203
		if (!IS_ERR(skb))
			dev_kfree_skb(skb);
204

205
		skb = ERR_PTR(-ENODEV);
206 207
	}

208
	dev->cb(dev->nfc_digital_dev, dev->arg, skb);
209 210
}

211 212
static int nfcsim_send(struct nfc_digital_dev *ddev, struct sk_buff *skb,
		       u16 timeout, nfc_digital_cmd_complete_t cb, void *arg)
213
{
214 215
	struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
	u8 delay;
216

217 218 219 220 221 222 223 224
	if (!dev->up) {
		NFCSIM_ERR(dev, "Device is down\n");
		return -ENODEV;
	}

	dev->recv_timeout = timeout;
	dev->cb = cb;
	dev->arg = arg;
225

226
	schedule_work(&dev->recv_work);
227

228 229 230 231 232 233 234 235
	if (dev->dropframe) {
		NFCSIM_DBG(dev, "dropping frame (out of %d)\n", dev->dropframe);
		dev_kfree_skb(skb);
		dev->dropframe--;

		return 0;
	}

236 237 238
	if (skb) {
		nfcsim_link_set_skb(dev->link_out, skb, dev->rf_tech,
				    dev->mode);
239

240 241 242
		/* Add random delay (between 3 and 10 ms) before sending data */
		get_random_bytes(&delay, 1);
		delay = 3 + (delay & 0x07);
243

244 245 246 247
		schedule_delayed_work(&dev->send_work, msecs_to_jiffies(delay));
	}

	return 0;
248 249
}

250
static void nfcsim_abort_cmd(struct nfc_digital_dev *ddev)
251
{
252
	struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
253

254
	nfcsim_link_recv_cancel(dev->link_in);
255 256
}

257
static int nfcsim_switch_rf(struct nfc_digital_dev *ddev, bool on)
258
{
259
	struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
260

261 262 263
	dev->up = on;

	return 0;
264 265
}

266 267
static int nfcsim_in_configure_hw(struct nfc_digital_dev *ddev,
					  int type, int param)
268
{
269
	struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
270

271 272 273 274 275 276
	switch (type) {
	case NFC_DIGITAL_CONFIG_RF_TECH:
		dev->up = true;
		dev->mode = NFCSIM_MODE_INITIATOR;
		dev->rf_tech = param;
		break;
277

278 279
	case NFC_DIGITAL_CONFIG_FRAMING:
		break;
280

281 282 283
	default:
		NFCSIM_ERR(dev, "Invalid configuration type: %d\n", type);
		return -EINVAL;
284 285
	}

286
	return 0;
287 288
}

289 290 291
static int nfcsim_in_send_cmd(struct nfc_digital_dev *ddev,
			       struct sk_buff *skb, u16 timeout,
			       nfc_digital_cmd_complete_t cb, void *arg)
292
{
293 294
	return nfcsim_send(ddev, skb, timeout, cb, arg);
}
295

296 297 298 299
static int nfcsim_tg_configure_hw(struct nfc_digital_dev *ddev,
					  int type, int param)
{
	struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
300

301 302 303 304 305 306
	switch (type) {
	case NFC_DIGITAL_CONFIG_RF_TECH:
		dev->up = true;
		dev->mode = NFCSIM_MODE_TARGET;
		dev->rf_tech = param;
		break;
307

308 309
	case NFC_DIGITAL_CONFIG_FRAMING:
		break;
310

311 312 313
	default:
		NFCSIM_ERR(dev, "Invalid configuration type: %d\n", type);
		return -EINVAL;
314 315
	}

316
	return 0;
317 318
}

319 320 321
static int nfcsim_tg_send_cmd(struct nfc_digital_dev *ddev,
			       struct sk_buff *skb, u16 timeout,
			       nfc_digital_cmd_complete_t cb, void *arg)
322
{
323
	return nfcsim_send(ddev, skb, timeout, cb, arg);
324 325
}

326 327
static int nfcsim_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
			    nfc_digital_cmd_complete_t cb, void *arg)
328
{
329
	return nfcsim_send(ddev, NULL, timeout, cb, arg);
330 331
}

332 333 334
static struct nfc_digital_ops nfcsim_digital_ops = {
	.in_configure_hw = nfcsim_in_configure_hw,
	.in_send_cmd = nfcsim_in_send_cmd,
335

336 337 338
	.tg_listen = nfcsim_tg_listen,
	.tg_configure_hw = nfcsim_tg_configure_hw,
	.tg_send_cmd = nfcsim_tg_send_cmd,
339

340 341 342
	.abort_cmd = nfcsim_abort_cmd,
	.switch_rf = nfcsim_switch_rf,
};
343

344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384
static struct dentry *nfcsim_debugfs_root;

static void nfcsim_debugfs_init(void)
{
	nfcsim_debugfs_root = debugfs_create_dir("nfcsim", NULL);

	if (!nfcsim_debugfs_root)
		pr_err("Could not create debugfs entry\n");

}

static void nfcsim_debugfs_remove(void)
{
	debugfs_remove_recursive(nfcsim_debugfs_root);
}

static void nfcsim_debugfs_init_dev(struct nfcsim *dev)
{
	struct dentry *dev_dir;
	char devname[5]; /* nfcX\0 */
	u32 idx;
	int n;

	if (!nfcsim_debugfs_root) {
		NFCSIM_ERR(dev, "nfcsim debugfs not initialized\n");
		return;
	}

	idx = dev->nfc_digital_dev->nfc_dev->idx;
	n = snprintf(devname, sizeof(devname), "nfc%d", idx);
	if (n >= sizeof(devname)) {
		NFCSIM_ERR(dev, "Could not compute dev name for dev %d\n", idx);
		return;
	}

	dev_dir = debugfs_create_dir(devname, nfcsim_debugfs_root);
	if (!dev_dir) {
		NFCSIM_ERR(dev, "Could not create debugfs entries for nfc%d\n",
			   idx);
		return;
	}
385 386

	debugfs_create_u8("dropframe", 0664, dev_dir, &dev->dropframe);
387 388
}

389 390
static struct nfcsim *nfcsim_device_new(struct nfcsim_link *link_in,
					struct nfcsim_link *link_out)
391
{
392 393
	struct nfcsim *dev;
	int rc;
394

395 396 397
	dev = kzalloc(sizeof(struct nfcsim), GFP_KERNEL);
	if (!dev)
		return ERR_PTR(-ENOMEM);
398

399 400
	INIT_DELAYED_WORK(&dev->send_work, nfcsim_send_wq);
	INIT_WORK(&dev->recv_work, nfcsim_recv_wq);
401

402 403 404 405 406 407 408 409
	dev->nfc_digital_dev =
			nfc_digital_allocate_device(&nfcsim_digital_ops,
						    NFC_PROTO_NFC_DEP_MASK,
						    NFCSIM_CAPABILITIES,
						    0, 0);
	if (!dev->nfc_digital_dev) {
		kfree(dev);
		return ERR_PTR(-ENOMEM);
410 411
	}

412
	nfc_digital_set_drvdata(dev->nfc_digital_dev, dev);
413

414 415
	dev->link_in = link_in;
	dev->link_out = link_out;
416

417 418 419 420 421
	rc = nfc_digital_register_device(dev->nfc_digital_dev);
	if (rc) {
		pr_err("Could not register digital device (%d)\n", rc);
		nfc_digital_free_device(dev->nfc_digital_dev);
		kfree(dev);
422

423
		return ERR_PTR(rc);
424 425
	}

426 427
	nfcsim_debugfs_init_dev(dev);

428
	return dev;
429 430
}

431
static void nfcsim_device_free(struct nfcsim *dev)
432
{
433
	nfc_digital_unregister_device(dev->nfc_digital_dev);
434

435
	dev->up = false;
436

437
	nfcsim_link_shutdown(dev->link_in);
438

439 440
	cancel_delayed_work_sync(&dev->send_work);
	cancel_work_sync(&dev->recv_work);
441

442
	nfc_digital_free_device(dev->nfc_digital_dev);
443 444 445 446

	kfree(dev);
}

447 448
static struct nfcsim *dev0;
static struct nfcsim *dev1;
449

450
static int __init nfcsim_init(void)
451
{
452
	struct nfcsim_link *link0, *link1;
453 454
	int rc;

455 456 457
	link0 = nfcsim_link_new();
	link1 = nfcsim_link_new();
	if (!link0 || !link1) {
458
		rc = -ENOMEM;
459
		goto exit_err;
460 461
	}

462 463
	nfcsim_debugfs_init();

464
	dev0 = nfcsim_device_new(link0, link1);
465 466
	if (IS_ERR(dev0)) {
		rc = PTR_ERR(dev0);
467
		goto exit_err;
468 469
	}

470
	dev1 = nfcsim_device_new(link1, link0);
471
	if (IS_ERR(dev1)) {
472
		nfcsim_device_free(dev0);
473 474

		rc = PTR_ERR(dev1);
475
		goto exit_err;
476 477
	}

478
	pr_info("nfcsim " NFCSIM_VERSION " initialized\n");
479

480 481 482 483
	return 0;

exit_err:
	pr_err("Failed to initialize nfcsim driver (%d)\n", rc);
484

485 486 487 488
	if (link0)
		nfcsim_link_free(link0);
	if (link1)
		nfcsim_link_free(link1);
489 490 491 492

	return rc;
}

493
static void __exit nfcsim_exit(void)
494
{
495 496 497 498
	struct nfcsim_link *link0, *link1;

	link0 = dev0->link_in;
	link1 = dev0->link_out;
499

500 501
	nfcsim_device_free(dev0);
	nfcsim_device_free(dev1);
502

503 504
	nfcsim_link_free(link0);
	nfcsim_link_free(link1);
505 506

	nfcsim_debugfs_remove();
507 508 509 510 511 512 513 514
}

module_init(nfcsim_init);
module_exit(nfcsim_exit);

MODULE_DESCRIPTION("NFCSim driver ver " NFCSIM_VERSION);
MODULE_VERSION(NFCSIM_VERSION);
MODULE_LICENSE("GPL");