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