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