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