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