linux/drivers/nfc/nfcsim.c
<<
>>
Prefs
   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
  31struct 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
  52struct 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
  65static struct nfcsim_link *nfcsim_link_new(void)
  66{
  67        struct nfcsim_link *link;
  68
  69        link = kzalloc(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
  79static void nfcsim_link_free(struct nfcsim_link *link)
  80{
  81        dev_kfree_skb(link->skb);
  82        kfree(link);
  83}
  84
  85static void nfcsim_link_recv_wake(struct nfcsim_link *link)
  86{
  87        link->cond = 1;
  88        wake_up_interruptible(&link->recv_wait);
  89}
  90
  91static 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(&link->lock);
 102}
 103
 104static 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(&link->lock);
 111
 112        nfcsim_link_recv_wake(link);
 113}
 114
 115static 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(&link->lock);
 123
 124        nfcsim_link_recv_wake(link);
 125}
 126
 127static 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
 157done:
 158        mutex_unlock(&link->lock);
 159
 160        if (rc < 0) {
 161                dev_kfree_skb(skb);
 162                skb = ERR_PTR(rc);
 163        }
 164
 165        link->cond = 0;
 166
 167        return skb;
 168}
 169
 170static 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(dev->link_out);
 180}
 181
 182static 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(dev->link_in, dev->recv_timeout,
 188                                   dev->rf_tech, dev->mode);
 189
 190        if (!dev->up) {
 191                NFCSIM_ERR(dev, "Device is down\n");
 192
 193                if (!IS_ERR(skb))
 194                        dev_kfree_skb(skb);
 195                return;
 196        }
 197
 198        dev->cb(dev->nfc_digital_dev, dev->arg, skb);
 199}
 200
 201static 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(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(&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(dev->link_out, skb, dev->rf_tech,
 228                                    dev->mode);
 229
 230                /* Add random delay (between 3 and 10 ms) before sending data */
 231                get_random_bytes(&delay, 1);
 232                delay = 3 + (delay & 0x07);
 233
 234                schedule_delayed_work(&dev->send_work, msecs_to_jiffies(delay));
 235        }
 236
 237        return 0;
 238}
 239
 240static void nfcsim_abort_cmd(struct nfc_digital_dev *ddev)
 241{
 242        struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
 243
 244        nfcsim_link_recv_cancel(dev->link_in);
 245}
 246
 247static int nfcsim_switch_rf(struct nfc_digital_dev *ddev, bool on)
 248{
 249        struct nfcsim *dev = nfc_digital_get_drvdata(ddev);
 250
 251        dev->up = on;
 252
 253        return 0;
 254}
 255
 256static int nfcsim_in_configure_hw(struct nfc_digital_dev *ddev,
 257                                          int type, int param)
 258{
 259        struct nfcsim *dev = nfc_digital_get_drvdata(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
 279static 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
 286static int nfcsim_tg_configure_hw(struct nfc_digital_dev *ddev,
 287                                          int type, int param)
 288{
 289        struct nfcsim *dev = nfc_digital_get_drvdata(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
 309static 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
 316static 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
 322static 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
 334static struct dentry *nfcsim_debugfs_root;
 335
 336static void nfcsim_debugfs_init(void)
 337{
 338        nfcsim_debugfs_root = debugfs_create_dir("nfcsim", NULL);
 339
 340        if (!nfcsim_debugfs_root)
 341                pr_err("Could not create debugfs entry\n");
 342
 343}
 344
 345static void nfcsim_debugfs_remove(void)
 346{
 347        debugfs_remove_recursive(nfcsim_debugfs_root);
 348}
 349
 350static void nfcsim_debugfs_init_dev(struct nfcsim *dev)
 351{
 352        struct dentry *dev_dir;
 353        char devname[5]; /* nfcX\0 */
 354        u32 idx;
 355        int n;
 356
 357        if (!nfcsim_debugfs_root) {
 358                NFCSIM_ERR(dev, "nfcsim debugfs not initialized\n");
 359                return;
 360        }
 361
 362        idx = dev->nfc_digital_dev->nfc_dev->idx;
 363        n = snprintf(devname, sizeof(devname), "nfc%d", idx);
 364        if (n >= sizeof(devname)) {
 365                NFCSIM_ERR(dev, "Could not compute dev name for dev %d\n", idx);
 366                return;
 367        }
 368
 369        dev_dir = debugfs_create_dir(devname, nfcsim_debugfs_root);
 370        if (!dev_dir) {
 371                NFCSIM_ERR(dev, "Could not create debugfs entries for nfc%d\n",
 372                           idx);
 373                return;
 374        }
 375
 376        debugfs_create_u8("dropframe", 0664, dev_dir, &dev->dropframe);
 377}
 378
 379static struct nfcsim *nfcsim_device_new(struct nfcsim_link *link_in,
 380                                        struct nfcsim_link *link_out)
 381{
 382        struct nfcsim *dev;
 383        int rc;
 384
 385        dev = kzalloc(sizeof(struct nfcsim), GFP_KERNEL);
 386        if (!dev)
 387                return ERR_PTR(-ENOMEM);
 388
 389        INIT_DELAYED_WORK(&dev->send_work, nfcsim_send_wq);
 390        INIT_WORK(&dev->recv_work, nfcsim_recv_wq);
 391
 392        dev->nfc_digital_dev =
 393                        nfc_digital_allocate_device(&nfcsim_digital_ops,
 394                                                    NFC_PROTO_NFC_DEP_MASK,
 395                                                    NFCSIM_CAPABILITIES,
 396                                                    0, 0);
 397        if (!dev->nfc_digital_dev) {
 398                kfree(dev);
 399                return ERR_PTR(-ENOMEM);
 400        }
 401
 402        nfc_digital_set_drvdata(dev->nfc_digital_dev, dev);
 403
 404        dev->link_in = link_in;
 405        dev->link_out = link_out;
 406
 407        rc = nfc_digital_register_device(dev->nfc_digital_dev);
 408        if (rc) {
 409                pr_err("Could not register digital device (%d)\n", rc);
 410                nfc_digital_free_device(dev->nfc_digital_dev);
 411                kfree(dev);
 412
 413                return ERR_PTR(rc);
 414        }
 415
 416        nfcsim_debugfs_init_dev(dev);
 417
 418        return dev;
 419}
 420
 421static void nfcsim_device_free(struct nfcsim *dev)
 422{
 423        nfc_digital_unregister_device(dev->nfc_digital_dev);
 424
 425        dev->up = false;
 426
 427        nfcsim_link_shutdown(dev->link_in);
 428
 429        cancel_delayed_work_sync(&dev->send_work);
 430        cancel_work_sync(&dev->recv_work);
 431
 432        nfc_digital_free_device(dev->nfc_digital_dev);
 433
 434        kfree(dev);
 435}
 436
 437static struct nfcsim *dev0;
 438static struct nfcsim *dev1;
 439
 440static int __init nfcsim_init(void)
 441{
 442        struct nfcsim_link *link0, *link1;
 443        int rc;
 444
 445        link0 = nfcsim_link_new();
 446        link1 = nfcsim_link_new();
 447        if (!link0 || !link1) {
 448                rc = -ENOMEM;
 449                goto exit_err;
 450        }
 451
 452        nfcsim_debugfs_init();
 453
 454        dev0 = nfcsim_device_new(link0, link1);
 455        if (IS_ERR(dev0)) {
 456                rc = PTR_ERR(dev0);
 457                goto exit_err;
 458        }
 459
 460        dev1 = nfcsim_device_new(link1, link0);
 461        if (IS_ERR(dev1)) {
 462                nfcsim_device_free(dev0);
 463
 464                rc = PTR_ERR(dev1);
 465                goto exit_err;
 466        }
 467
 468        pr_info("nfcsim " NFCSIM_VERSION " initialized\n");
 469
 470        return 0;
 471
 472exit_err:
 473        pr_err("Failed to initialize nfcsim driver (%d)\n", rc);
 474
 475        if (link0)
 476                nfcsim_link_free(link0);
 477        if (link1)
 478                nfcsim_link_free(link1);
 479
 480        return rc;
 481}
 482
 483static void __exit nfcsim_exit(void)
 484{
 485        struct nfcsim_link *link0, *link1;
 486
 487        link0 = dev0->link_in;
 488        link1 = dev0->link_out;
 489
 490        nfcsim_device_free(dev0);
 491        nfcsim_device_free(dev1);
 492
 493        nfcsim_link_free(link0);
 494        nfcsim_link_free(link1);
 495
 496        nfcsim_debugfs_remove();
 497}
 498
 499module_init(nfcsim_init);
 500module_exit(nfcsim_exit);
 501
 502MODULE_DESCRIPTION("NFCSim driver ver " NFCSIM_VERSION);
 503MODULE_VERSION(NFCSIM_VERSION);
 504MODULE_LICENSE("GPL");
 505