drivers/net/wwan/wwan_hwsim.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * WWAN device simulator for WWAN framework testing.
   4  *
   5  * Copyright (c) 2021, Sergey Ryazanov <ryazanov.s.a@gmail.com>
   6  */
   7
   8 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   9
  10 #include <linux/kernel.h>
  11 #include <linux/module.h>
  12 #include <linux/slab.h>
  13 #include <linux/device.h>
  14 #include <linux/spinlock.h>
  15 #include <linux/list.h>
  16 #include <linux/skbuff.h>
  17 #include <linux/netdevice.h>
  18 #include <linux/wwan.h>
  19 #include <linux/debugfs.h>
  20 #include <linux/workqueue.h>
  21
  22 #include <net/arp.h>
  23
  24 static int wwan_hwsim_devsnum = 2;
  25 module_param_named(devices, wwan_hwsim_devsnum, int, 0444);
  26 MODULE_PARM_DESC(devices, "Number of simulated devices");
  27
  28 static struct class *wwan_hwsim_class;
  29
  30 static struct dentry *wwan_hwsim_debugfs_topdir;
  31 static struct dentry *wwan_hwsim_debugfs_devcreate;
  32
  33 static DEFINE_SPINLOCK(wwan_hwsim_devs_lock);
  34 static LIST_HEAD(wwan_hwsim_devs);
  35 static unsigned int wwan_hwsim_dev_idx;
  36 static struct workqueue_struct *wwan_wq;
  37
  38 struct wwan_hwsim_dev {
  39         struct list_head list;
  40         unsigned int id;
  41         struct device dev;
  42         struct work_struct del_work;
  43         struct dentry *debugfs_topdir;
  44         struct dentry *debugfs_portcreate;
  45         spinlock_t ports_lock;  /* Serialize ports creation/deletion */
  46         unsigned int port_idx;
  47         struct list_head ports;
  48 };
  49
  50 struct wwan_hwsim_port {
  51         struct list_head list;
  52         unsigned int id;
  53         struct wwan_hwsim_dev *dev;
  54         struct wwan_port *wwan;
  55         struct work_struct del_work;
  56         struct dentry *debugfs_topdir;
  57         enum {                  /* AT command parser state */
  58                 AT_PARSER_WAIT_A,
  59                 AT_PARSER_WAIT_T,
  60                 AT_PARSER_WAIT_TERM,
  61                 AT_PARSER_SKIP_LINE,
  62         } pstate;
  63 };
  64
  65 static const struct file_operations wwan_hwsim_debugfs_portdestroy_fops;
  66 static const struct file_operations wwan_hwsim_debugfs_portcreate_fops;
  67 static const struct file_operations wwan_hwsim_debugfs_devdestroy_fops;
  68 static void wwan_hwsim_port_del_work(struct work_struct *work);
  69 static void wwan_hwsim_dev_del_work(struct work_struct *work);
  70
  71 static netdev_tx_t wwan_hwsim_netdev_xmit(struct sk_buff *skb,
  72                                           struct net_device *ndev)
  73 {
  74         ndev->stats.tx_packets++;
  75         ndev->stats.tx_bytes += skb->len;
  76         consume_skb(skb);
  77         return NETDEV_TX_OK;
  78 }
  79
  80 static const struct net_device_ops wwan_hwsim_netdev_ops = {
  81         .ndo_start_xmit = wwan_hwsim_netdev_xmit,
  82 };
  83
  84 static void wwan_hwsim_netdev_setup(struct net_device *ndev)
  85 {
  86         ndev->netdev_ops = &wwan_hwsim_netdev_ops;
  87         ndev->needs_free_netdev = true;
  88
  89         ndev->mtu = ETH_DATA_LEN;
  90         ndev->min_mtu = ETH_MIN_MTU;
  91         ndev->max_mtu = ETH_MAX_MTU;
  92
  93         ndev->type = ARPHRD_NONE;
  94         ndev->flags = IFF_POINTOPOINT | IFF_NOARP;
  95 }
  96
  97 static const struct wwan_ops wwan_hwsim_wwan_rtnl_ops = {
  98         .priv_size = 0,                 /* No private data */
  99         .setup = wwan_hwsim_netdev_setup,
 100 };
 101
 102 static int wwan_hwsim_port_start(struct wwan_port *wport)
 103 {
 104         struct wwan_hwsim_port *port = wwan_port_get_drvdata(wport);
 105
 106         port->pstate = AT_PARSER_WAIT_A;
 107
 108         return 0;
 109 }
 110
 111 static void wwan_hwsim_port_stop(struct wwan_port *wport)
 112 {
 113 }
 114
 115 /* Implements a minimalistic AT commands parser that echo input back and
 116  * reply with 'OK' to each input command. See AT command protocol details in the
 117  * ITU-T V.250 recomendations document.
 118  *
 119  * Be aware that this processor is not fully V.250 compliant.
 120  */
 121 static int wwan_hwsim_port_tx(struct wwan_port *wport, struct sk_buff *in)
 122 {
 123         struct wwan_hwsim_port *port = wwan_port_get_drvdata(wport);
 124         struct sk_buff *out;
 125         int i, n, s;
 126
 127         /* Estimate a max possible number of commands by counting the number of
 128          * termination chars (S3 param, CR by default). And then allocate the
 129          * output buffer that will be enough to fit the echo and result codes of
 130          * all commands.
 131          */
 132         for (i = 0, n = 0; i < in->len; ++i)
 133                 if (in->data[i] == '\r')
 134                         n++;
 135         n = in->len + n * (2 + 2 + 2);  /* Output buffer size */
 136         out = alloc_skb(n, GFP_KERNEL);
 137         if (!out)
 138                 return -ENOMEM;
 139
 140         for (i = 0, s = 0; i < in->len; ++i) {
 141                 char c = in->data[i];
 142
 143                 if (port->pstate == AT_PARSER_WAIT_A) {
 144                         if (c == 'A' || c == 'a')
 145                                 port->pstate = AT_PARSER_WAIT_T;
 146                         else if (c != '\n')     /* Ignore formating char */
 147                                 port->pstate = AT_PARSER_SKIP_LINE;
 148                 } else if (port->pstate == AT_PARSER_WAIT_T) {
 149                         if (c == 'T' || c == 't')
 150                                 port->pstate = AT_PARSER_WAIT_TERM;
 151                         else
 152                                 port->pstate = AT_PARSER_SKIP_LINE;
 153                 } else if (port->pstate == AT_PARSER_WAIT_TERM) {
 154                         if (c != '\r')
 155                                 continue;
 156                         /* Consume the trailing formatting char as well */
 157                         if ((i + 1) < in->len && in->data[i + 1] == '\n')
 158                                 i++;
 159                         n = i - s + 1;
 160                         skb_put_data(out, &in->data[s], n);/* Echo */
 161                         skb_put_data(out, "\r\nOK\r\n", 6);
 162                         s = i + 1;
 163                         port->pstate = AT_PARSER_WAIT_A;
 164                 } else if (port->pstate == AT_PARSER_SKIP_LINE) {
 165                         if (c != '\r')
 166                                 continue;
 167                         port->pstate = AT_PARSER_WAIT_A;
 168                 }
 169         }
 170
 171         if (i > s) {
 172                 /* Echo the processed portion of a not yet completed command */
 173                 n = i - s;
 174                 skb_put_data(out, &in->data[s], n);
 175         }
 176
 177         consume_skb(in);
 178
 179         wwan_port_rx(wport, out);
 180
 181         return 0;
 182 }
 183
 184 static const struct wwan_port_ops wwan_hwsim_port_ops = {
 185         .start = wwan_hwsim_port_start,
 186         .stop = wwan_hwsim_port_stop,
 187         .tx = wwan_hwsim_port_tx,
 188 };
 189
 190 static struct wwan_hwsim_port *wwan_hwsim_port_new(struct wwan_hwsim_dev *dev)
 191 {
 192         struct wwan_hwsim_port *port;
 193         char name[0x10];
 194         int err;
 195
 196         port = kzalloc(sizeof(*port), GFP_KERNEL);
 197         if (!port)
 198                 return ERR_PTR(-ENOMEM);
 199
 200         port->dev = dev;
 201
 202         spin_lock(&dev->ports_lock);
 203         port->id = dev->port_idx++;
 204         spin_unlock(&dev->ports_lock);
 205
 206         port->wwan = wwan_create_port(&dev->dev, WWAN_PORT_AT,
 207                                       &wwan_hwsim_port_ops,
 208                                       port);
 209         if (IS_ERR(port->wwan)) {
 210                 err = PTR_ERR(port->wwan);
 211                 goto err_free_port;
 212         }
 213
 214         INIT_WORK(&port->del_work, wwan_hwsim_port_del_work);
 215
 216         snprintf(name, sizeof(name), "port%u", port->id);
 217         port->debugfs_topdir = debugfs_create_dir(name, dev->debugfs_topdir);
 218         debugfs_create_file("destroy", 0200, port->debugfs_topdir, port,
 219                             &wwan_hwsim_debugfs_portdestroy_fops);
 220
 221         return port;
 222
 223 err_free_port:
 224         kfree(port);
 225
 226         return ERR_PTR(err);
 227 }
 228
 229 static void wwan_hwsim_port_del(struct wwan_hwsim_port *port)
 230 {
 231         debugfs_remove(port->debugfs_topdir);
 232
 233         /* Make sure that there is no pending deletion work */
 234         if (current_work() != &port->del_work)
 235                 cancel_work_sync(&port->del_work);
 236
 237         wwan_remove_port(port->wwan);
 238         kfree(port);
 239 }
 240
 241 static void wwan_hwsim_port_del_work(struct work_struct *work)
 242 {
 243         struct wwan_hwsim_port *port =
 244                                 container_of(work, typeof(*port), del_work);
 245         struct wwan_hwsim_dev *dev = port->dev;
 246
 247         spin_lock(&dev->ports_lock);
 248         if (list_empty(&port->list)) {
 249                 /* Someone else deleting port at the moment */
 250                 spin_unlock(&dev->ports_lock);
 251                 return;
 252         }
 253         list_del_init(&port->list);
 254         spin_unlock(&dev->ports_lock);
 255
 256         wwan_hwsim_port_del(port);
 257 }
 258
 259 static void wwan_hwsim_dev_release(struct device *sysdev)
 260 {
 261         struct wwan_hwsim_dev *dev = container_of(sysdev, typeof(*dev), dev);
 262
 263         kfree(dev);
 264 }
 265
 266 static struct wwan_hwsim_dev *wwan_hwsim_dev_new(void)
 267 {
 268         struct wwan_hwsim_dev *dev;
 269         int err;
 270
 271         dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 272         if (!dev)
 273                 return ERR_PTR(-ENOMEM);
 274
 275         spin_lock(&wwan_hwsim_devs_lock);
 276         dev->id = wwan_hwsim_dev_idx++;
 277         spin_unlock(&wwan_hwsim_devs_lock);
 278
 279         dev->dev.release = wwan_hwsim_dev_release;
 280         dev->dev.class = wwan_hwsim_class;
 281         dev_set_name(&dev->dev, "hwsim%u", dev->id);
 282
 283         spin_lock_init(&dev->ports_lock);
 284         INIT_LIST_HEAD(&dev->ports);
 285
 286         err = device_register(&dev->dev);
 287         if (err)
 288                 goto err_free_dev;
 289
 290         INIT_WORK(&dev->del_work, wwan_hwsim_dev_del_work);
 291
 292         err = wwan_register_ops(&dev->dev, &wwan_hwsim_wwan_rtnl_ops, dev, 1);
 293         if (err)
 294                 goto err_unreg_dev;
 295
 296         dev->debugfs_topdir = debugfs_create_dir(dev_name(&dev->dev),
 297                                                  wwan_hwsim_debugfs_topdir);
 298         debugfs_create_file("destroy", 0200, dev->debugfs_topdir, dev,
 299                             &wwan_hwsim_debugfs_devdestroy_fops);
 300         dev->debugfs_portcreate =
 301                 debugfs_create_file("portcreate", 0200,
 302                                     dev->debugfs_topdir, dev,
 303                                     &wwan_hwsim_debugfs_portcreate_fops);
 304
 305         return dev;
 306
 307 err_unreg_dev:
 308         device_unregister(&dev->dev);
 309         /* Memory will be freed in the device release callback */
 310
 311         return ERR_PTR(err);
 312
 313 err_free_dev:
 314         put_device(&dev->dev);
 315
 316         return ERR_PTR(err);
 317 }
 318
 319 static void wwan_hwsim_dev_del(struct wwan_hwsim_dev *dev)
 320 {
 321         debugfs_remove(dev->debugfs_portcreate);        /* Avoid new ports */
 322
 323         spin_lock(&dev->ports_lock);
 324         while (!list_empty(&dev->ports)) {
 325                 struct wwan_hwsim_port *port;
 326
 327                 port = list_first_entry(&dev->ports, struct wwan_hwsim_port,
 328                                         list);
 329                 list_del_init(&port->list);
 330                 spin_unlock(&dev->ports_lock);
 331                 wwan_hwsim_port_del(port);
 332                 spin_lock(&dev->ports_lock);
 333         }
 334         spin_unlock(&dev->ports_lock);
 335
 336         debugfs_remove(dev->debugfs_topdir);
 337
 338         /* This will remove all child netdev(s) */
 339         wwan_unregister_ops(&dev->dev);
 340
 341         /* Make sure that there is no pending deletion work */
 342         if (current_work() != &dev->del_work)
 343                 cancel_work_sync(&dev->del_work);
 344
 345         device_unregister(&dev->dev);
 346         /* Memory will be freed in the device release callback */
 347 }
 348
 349 static void wwan_hwsim_dev_del_work(struct work_struct *work)
 350 {
 351         struct wwan_hwsim_dev *dev = container_of(work, typeof(*dev), del_work);
 352
 353         spin_lock(&wwan_hwsim_devs_lock);
 354         if (list_empty(&dev->list)) {
 355                 /* Someone else deleting device at the moment */
 356                 spin_unlock(&wwan_hwsim_devs_lock);
 357                 return;
 358         }
 359         list_del_init(&dev->list);
 360         spin_unlock(&wwan_hwsim_devs_lock);
 361
 362         wwan_hwsim_dev_del(dev);
 363 }
 364
 365 static ssize_t wwan_hwsim_debugfs_portdestroy_write(struct file *file,
 366                                                     const char __user *usrbuf,
 367                                                     size_t count, loff_t *ppos)
 368 {
 369         struct wwan_hwsim_port *port = file->private_data;
 370
 371         /* We can not delete port here since it will cause a deadlock due to
 372          * waiting this callback to finish in the debugfs_remove() call. So,
 373          * use workqueue.
 374          */
 375         queue_work(wwan_wq, &port->del_work);
 376
 377         return count;
 378 }
 379
 380 static const struct file_operations wwan_hwsim_debugfs_portdestroy_fops = {
 381         .write = wwan_hwsim_debugfs_portdestroy_write,
 382         .open = simple_open,
 383         .llseek = noop_llseek,
 384 };
 385
 386 static ssize_t wwan_hwsim_debugfs_portcreate_write(struct file *file,
 387                                                    const char __user *usrbuf,
 388                                                    size_t count, loff_t *ppos)
 389 {
 390         struct wwan_hwsim_dev *dev = file->private_data;
 391         struct wwan_hwsim_port *port;
 392
 393         port = wwan_hwsim_port_new(dev);
 394         if (IS_ERR(port))
 395                 return PTR_ERR(port);
 396
 397         spin_lock(&dev->ports_lock);
 398         list_add_tail(&port->list, &dev->ports);
 399         spin_unlock(&dev->ports_lock);
 400
 401         return count;
 402 }
 403
 404 static const struct file_operations wwan_hwsim_debugfs_portcreate_fops = {
 405         .write = wwan_hwsim_debugfs_portcreate_write,
 406         .open = simple_open,
 407         .llseek = noop_llseek,
 408 };
 409
 410 static ssize_t wwan_hwsim_debugfs_devdestroy_write(struct file *file,
 411                                                    const char __user *usrbuf,
 412                                                    size_t count, loff_t *ppos)
 413 {
 414         struct wwan_hwsim_dev *dev = file->private_data;
 415
 416         /* We can not delete device here since it will cause a deadlock due to
 417          * waiting this callback to finish in the debugfs_remove() call. So,
 418          * use workqueue.
 419          */
 420         queue_work(wwan_wq, &dev->del_work);
 421
 422         return count;
 423 }
 424
 425 static const struct file_operations wwan_hwsim_debugfs_devdestroy_fops = {
 426         .write = wwan_hwsim_debugfs_devdestroy_write,
 427         .open = simple_open,
 428         .llseek = noop_llseek,
 429 };
 430
 431 static ssize_t wwan_hwsim_debugfs_devcreate_write(struct file *file,
 432                                                   const char __user *usrbuf,
 433                                                   size_t count, loff_t *ppos)
 434 {
 435         struct wwan_hwsim_dev *dev;
 436
 437         dev = wwan_hwsim_dev_new();
 438         if (IS_ERR(dev))
 439                 return PTR_ERR(dev);
 440
 441         spin_lock(&wwan_hwsim_devs_lock);
 442         list_add_tail(&dev->list, &wwan_hwsim_devs);
 443         spin_unlock(&wwan_hwsim_devs_lock);
 444
 445         return count;
 446 }
 447
 448 static const struct file_operations wwan_hwsim_debugfs_devcreate_fops = {
 449         .write = wwan_hwsim_debugfs_devcreate_write,
 450         .open = simple_open,
 451         .llseek = noop_llseek,
 452 };
 453
 454 static int __init wwan_hwsim_init_devs(void)
 455 {
 456         struct wwan_hwsim_dev *dev;
 457         int i, j;
 458
 459         for (i = 0; i < wwan_hwsim_devsnum; ++i) {
 460                 dev = wwan_hwsim_dev_new();
 461                 if (IS_ERR(dev))
 462                         return PTR_ERR(dev);
 463
 464                 spin_lock(&wwan_hwsim_devs_lock);
 465                 list_add_tail(&dev->list, &wwan_hwsim_devs);
 466                 spin_unlock(&wwan_hwsim_devs_lock);
 467
 468                 /* Create a couple of ports per each device to accelerate
 469                  * the simulator readiness time.
 470                  */
 471                 for (j = 0; j < 2; ++j) {
 472                         struct wwan_hwsim_port *port;
 473
 474                         port = wwan_hwsim_port_new(dev);
 475                         if (IS_ERR(port))
 476                                 return PTR_ERR(port);
 477
 478                         spin_lock(&dev->ports_lock);
 479                         list_add_tail(&port->list, &dev->ports);
 480                         spin_unlock(&dev->ports_lock);
 481                 }
 482         }
 483
 484         return 0;
 485 }
 486
 487 static void wwan_hwsim_free_devs(void)
 488 {
 489         struct wwan_hwsim_dev *dev;
 490
 491         spin_lock(&wwan_hwsim_devs_lock);
 492         while (!list_empty(&wwan_hwsim_devs)) {
 493                 dev = list_first_entry(&wwan_hwsim_devs, struct wwan_hwsim_dev,
 494                                        list);
 495                 list_del_init(&dev->list);
 496                 spin_unlock(&wwan_hwsim_devs_lock);
 497                 wwan_hwsim_dev_del(dev);
 498                 spin_lock(&wwan_hwsim_devs_lock);
 499         }
 500         spin_unlock(&wwan_hwsim_devs_lock);
 501 }
 502
 503 static int __init wwan_hwsim_init(void)
 504 {
 505         int err;
 506
 507         if (wwan_hwsim_devsnum < 0 || wwan_hwsim_devsnum > 128)
 508                 return -EINVAL;
 509
 510         wwan_wq = alloc_workqueue("wwan_wq", 0, 0);
 511         if (!wwan_wq)
 512                 return -ENOMEM;
 513
 514         wwan_hwsim_class = class_create(THIS_MODULE, "wwan_hwsim");
 515         if (IS_ERR(wwan_hwsim_class)) {
 516                 err = PTR_ERR(wwan_hwsim_class);
 517                 goto err_wq_destroy;
 518         }
 519
 520         wwan_hwsim_debugfs_topdir = debugfs_create_dir("wwan_hwsim", NULL);
 521         wwan_hwsim_debugfs_devcreate =
 522                         debugfs_create_file("devcreate", 0200,
 523                                             wwan_hwsim_debugfs_topdir, NULL,
 524                                             &wwan_hwsim_debugfs_devcreate_fops);
 525
 526         err = wwan_hwsim_init_devs();
 527         if (err)
 528                 goto err_clean_devs;
 529
 530         return 0;
 531
 532 err_clean_devs:
 533         debugfs_remove(wwan_hwsim_debugfs_devcreate);   /* Avoid new devs */
 534         wwan_hwsim_free_devs();
 535         flush_workqueue(wwan_wq);       /* Wait deletion works completion */
 536         debugfs_remove(wwan_hwsim_debugfs_topdir);
 537         class_destroy(wwan_hwsim_class);
 538 err_wq_destroy:
 539         destroy_workqueue(wwan_wq);
 540
 541         return err;
 542 }
 543
 544 static void __exit wwan_hwsim_exit(void)
 545 {
 546         debugfs_remove(wwan_hwsim_debugfs_devcreate);   /* Avoid new devs */
 547         wwan_hwsim_free_devs();
 548         flush_workqueue(wwan_wq);       /* Wait deletion works completion */
 549         debugfs_remove(wwan_hwsim_debugfs_topdir);
 550         class_destroy(wwan_hwsim_class);
 551         destroy_workqueue(wwan_wq);
 552 }
 553
 554 module_init(wwan_hwsim_init);
 555 module_exit(wwan_hwsim_exit);
 556
 557 MODULE_AUTHOR("Sergey Ryazanov");
 558 MODULE_DESCRIPTION("Device simulator for WWAN framework");
 559 MODULE_LICENSE("GPL");