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(port: 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(port: 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(size: 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(skb: out, data: &in->data[s], len: n);/* Echo */ |
163 | skb_put_data(skb: out, data: "\r\nOK\r\n" , len: 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(skb: out, data: &in->data[s], len: n); |
177 | } |
178 | |
179 | consume_skb(skb: in); |
180 | |
181 | wwan_port_rx(port: wport, skb: 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(size: sizeof(*port), GFP_KERNEL); |
199 | if (!port) |
200 | return ERR_PTR(error: -ENOMEM); |
201 | |
202 | port->dev = dev; |
203 | |
204 | spin_lock(lock: &dev->ports_lock); |
205 | port->id = dev->port_idx++; |
206 | spin_unlock(lock: &dev->ports_lock); |
207 | |
208 | port->wwan = wwan_create_port(parent: &dev->dev, type: WWAN_PORT_AT, |
209 | ops: &wwan_hwsim_port_ops, |
210 | NULL, drvdata: port); |
211 | if (IS_ERR(ptr: port->wwan)) { |
212 | err = PTR_ERR(ptr: port->wwan); |
213 | goto err_free_port; |
214 | } |
215 | |
216 | INIT_WORK(&port->del_work, wwan_hwsim_port_del_work); |
217 | |
218 | snprintf(buf: name, size: sizeof(name), fmt: "port%u" , port->id); |
219 | port->debugfs_topdir = debugfs_create_dir(name, parent: dev->debugfs_topdir); |
220 | debugfs_create_file(name: "destroy" , mode: 0200, parent: port->debugfs_topdir, data: port, |
221 | fops: &wwan_hwsim_debugfs_portdestroy_fops); |
222 | |
223 | return port; |
224 | |
225 | err_free_port: |
226 | kfree(objp: port); |
227 | |
228 | return ERR_PTR(error: err); |
229 | } |
230 | |
231 | static void wwan_hwsim_port_del(struct wwan_hwsim_port *port) |
232 | { |
233 | debugfs_remove(dentry: 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(work: &port->del_work); |
238 | |
239 | wwan_remove_port(port: port->wwan); |
240 | kfree(objp: 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(lock: &dev->ports_lock); |
250 | if (list_empty(head: &port->list)) { |
251 | /* Someone else deleting port at the moment */ |
252 | spin_unlock(lock: &dev->ports_lock); |
253 | return; |
254 | } |
255 | list_del_init(entry: &port->list); |
256 | spin_unlock(lock: &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(objp: 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(size: sizeof(*dev), GFP_KERNEL); |
274 | if (!dev) |
275 | return ERR_PTR(error: -ENOMEM); |
276 | |
277 | spin_lock(lock: &wwan_hwsim_devs_lock); |
278 | dev->id = wwan_hwsim_dev_idx++; |
279 | spin_unlock(lock: &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->dev, name: "hwsim%u" , dev->id); |
284 | |
285 | spin_lock_init(&dev->ports_lock); |
286 | INIT_LIST_HEAD(list: &dev->ports); |
287 | |
288 | err = device_register(dev: &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(parent: &dev->dev, ops: &wwan_hwsim_wwan_rtnl_ops, ctxt: dev, def_link_id: 1); |
295 | if (err) |
296 | goto err_unreg_dev; |
297 | |
298 | dev->debugfs_topdir = debugfs_create_dir(name: dev_name(dev: &dev->dev), |
299 | parent: wwan_hwsim_debugfs_topdir); |
300 | debugfs_create_file(name: "destroy" , mode: 0200, parent: dev->debugfs_topdir, data: dev, |
301 | fops: &wwan_hwsim_debugfs_devdestroy_fops); |
302 | dev->debugfs_portcreate = |
303 | debugfs_create_file(name: "portcreate" , mode: 0200, |
304 | parent: dev->debugfs_topdir, data: dev, |
305 | fops: &wwan_hwsim_debugfs_portcreate_fops); |
306 | |
307 | return dev; |
308 | |
309 | err_unreg_dev: |
310 | device_unregister(dev: &dev->dev); |
311 | /* Memory will be freed in the device release callback */ |
312 | |
313 | return ERR_PTR(error: err); |
314 | |
315 | err_free_dev: |
316 | put_device(dev: &dev->dev); |
317 | |
318 | return ERR_PTR(error: err); |
319 | } |
320 | |
321 | static void wwan_hwsim_dev_del(struct wwan_hwsim_dev *dev) |
322 | { |
323 | debugfs_remove(dentry: dev->debugfs_portcreate); /* Avoid new ports */ |
324 | |
325 | spin_lock(lock: &dev->ports_lock); |
326 | while (!list_empty(head: &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(entry: &port->list); |
332 | spin_unlock(lock: &dev->ports_lock); |
333 | wwan_hwsim_port_del(port); |
334 | spin_lock(lock: &dev->ports_lock); |
335 | } |
336 | spin_unlock(lock: &dev->ports_lock); |
337 | |
338 | debugfs_remove(dentry: dev->debugfs_topdir); |
339 | |
340 | /* This will remove all child netdev(s) */ |
341 | wwan_unregister_ops(parent: &dev->dev); |
342 | |
343 | /* Make sure that there is no pending deletion work */ |
344 | if (current_work() != &dev->del_work) |
345 | cancel_work_sync(work: &dev->del_work); |
346 | |
347 | device_unregister(dev: &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(lock: &wwan_hwsim_devs_lock); |
356 | if (list_empty(head: &dev->list)) { |
357 | /* Someone else deleting device at the moment */ |
358 | spin_unlock(lock: &wwan_hwsim_devs_lock); |
359 | return; |
360 | } |
361 | list_del_init(entry: &dev->list); |
362 | spin_unlock(lock: &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(wq: wwan_wq, work: &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(ptr: port)) |
397 | return PTR_ERR(ptr: port); |
398 | |
399 | spin_lock(lock: &dev->ports_lock); |
400 | list_add_tail(new: &port->list, head: &dev->ports); |
401 | spin_unlock(lock: &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(wq: wwan_wq, work: &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(ptr: dev)) |
441 | return PTR_ERR(ptr: dev); |
442 | |
443 | spin_lock(lock: &wwan_hwsim_devs_lock); |
444 | list_add_tail(new: &dev->list, head: &wwan_hwsim_devs); |
445 | spin_unlock(lock: &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(ptr: dev)) |
464 | return PTR_ERR(ptr: dev); |
465 | |
466 | spin_lock(lock: &wwan_hwsim_devs_lock); |
467 | list_add_tail(new: &dev->list, head: &wwan_hwsim_devs); |
468 | spin_unlock(lock: &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(ptr: port)) |
478 | return PTR_ERR(ptr: port); |
479 | |
480 | spin_lock(lock: &dev->ports_lock); |
481 | list_add_tail(new: &port->list, head: &dev->ports); |
482 | spin_unlock(lock: &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(lock: &wwan_hwsim_devs_lock); |
494 | while (!list_empty(head: &wwan_hwsim_devs)) { |
495 | dev = list_first_entry(&wwan_hwsim_devs, struct wwan_hwsim_dev, |
496 | list); |
497 | list_del_init(entry: &dev->list); |
498 | spin_unlock(lock: &wwan_hwsim_devs_lock); |
499 | wwan_hwsim_dev_del(dev); |
500 | spin_lock(lock: &wwan_hwsim_devs_lock); |
501 | } |
502 | spin_unlock(lock: &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(fmt: "wwan_wq" , flags: 0, max_active: 0); |
513 | if (!wwan_wq) |
514 | return -ENOMEM; |
515 | |
516 | err = class_register(class: &wwan_hwsim_class); |
517 | if (err) |
518 | goto err_wq_destroy; |
519 | |
520 | wwan_hwsim_debugfs_topdir = debugfs_create_dir(name: "wwan_hwsim" , NULL); |
521 | wwan_hwsim_debugfs_devcreate = |
522 | debugfs_create_file(name: "devcreate" , mode: 0200, |
523 | parent: wwan_hwsim_debugfs_topdir, NULL, |
524 | fops: &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(dentry: wwan_hwsim_debugfs_devcreate); /* Avoid new devs */ |
534 | wwan_hwsim_free_devs(); |
535 | flush_workqueue(wwan_wq); /* Wait deletion works completion */ |
536 | debugfs_remove(dentry: wwan_hwsim_debugfs_topdir); |
537 | class_unregister(class: &wwan_hwsim_class); |
538 | err_wq_destroy: |
539 | destroy_workqueue(wq: wwan_wq); |
540 | |
541 | return err; |
542 | } |
543 | |
544 | static void __exit wwan_hwsim_exit(void) |
545 | { |
546 | debugfs_remove(dentry: wwan_hwsim_debugfs_devcreate); /* Avoid new devs */ |
547 | wwan_hwsim_free_devs(); |
548 | flush_workqueue(wwan_wq); /* Wait deletion works completion */ |
549 | debugfs_remove(dentry: wwan_hwsim_debugfs_topdir); |
550 | class_unregister(class: &wwan_hwsim_class); |
551 | destroy_workqueue(wq: 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 | |