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 memcpy(skb_put(out, n), &in->data[s], n);/* Echo */ 161 memcpy(skb_put(out, 6), "\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 memcpy(skb_put(out, n), &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 kfree(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"); 560