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 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"); 560