From f36a111a74e71edbba27d4c0cf3d7bbccc172108 Mon Sep 17 00:00:00 2001 From: Sergey Ryazanov Date: Tue, 8 Jun 2021 07:02:32 +0300 Subject: [PATCH] wwan_hwsim: WWAN device simulator This driver simulates a set of WWAN device with a set of AT control ports. It can be used to test WWAN kernel framework as well as user space tools. Signed-off-by: Sergey Ryazanov Signed-off-by: David S. Miller --- drivers/net/wwan/Kconfig | 10 ++ drivers/net/wwan/Makefile | 2 + drivers/net/wwan/wwan_hwsim.c | 318 ++++++++++++++++++++++++++++++++++ 3 files changed, 330 insertions(+) create mode 100644 drivers/net/wwan/wwan_hwsim.c diff --git a/drivers/net/wwan/Kconfig b/drivers/net/wwan/Kconfig index 7ad1920120bc..ec0b194a373c 100644 --- a/drivers/net/wwan/Kconfig +++ b/drivers/net/wwan/Kconfig @@ -20,6 +20,16 @@ config WWAN_CORE To compile this driver as a module, choose M here: the module will be called wwan. +config WWAN_HWSIM + tristate "Simulated WWAN device" + depends on WWAN_CORE + help + This driver is a developer testing tool that can be used to test WWAN + framework. + + To compile this driver as a module, choose M here: the module will be + called wwan_hwsim. If unsure, say N. + config MHI_WWAN_CTRL tristate "MHI WWAN control driver for QCOM-based PCIe modems" select WWAN_CORE diff --git a/drivers/net/wwan/Makefile b/drivers/net/wwan/Makefile index 556cd90958ca..f33f77ca1021 100644 --- a/drivers/net/wwan/Makefile +++ b/drivers/net/wwan/Makefile @@ -6,4 +6,6 @@ obj-$(CONFIG_WWAN_CORE) += wwan.o wwan-objs += wwan_core.o +obj-$(CONFIG_WWAN_HWSIM) += wwan_hwsim.o + obj-$(CONFIG_MHI_WWAN_CTRL) += mhi_wwan_ctrl.o diff --git a/drivers/net/wwan/wwan_hwsim.c b/drivers/net/wwan/wwan_hwsim.c new file mode 100644 index 000000000000..96d25d7e5bb8 --- /dev/null +++ b/drivers/net/wwan/wwan_hwsim.c @@ -0,0 +1,318 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * WWAN device simulator for WWAN framework testing. + * + * Copyright (c) 2021, Sergey Ryazanov + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include +#include +#include +#include +#include +#include +#include +#include + +static int wwan_hwsim_devsnum = 2; +module_param_named(devices, wwan_hwsim_devsnum, int, 0444); +MODULE_PARM_DESC(devices, "Number of simulated devices"); + +static struct class *wwan_hwsim_class; + +static DEFINE_SPINLOCK(wwan_hwsim_devs_lock); +static LIST_HEAD(wwan_hwsim_devs); +static unsigned int wwan_hwsim_dev_idx; + +struct wwan_hwsim_dev { + struct list_head list; + unsigned int id; + struct device dev; + spinlock_t ports_lock; /* Serialize ports creation/deletion */ + unsigned int port_idx; + struct list_head ports; +}; + +struct wwan_hwsim_port { + struct list_head list; + unsigned int id; + struct wwan_hwsim_dev *dev; + struct wwan_port *wwan; + enum { /* AT command parser state */ + AT_PARSER_WAIT_A, + AT_PARSER_WAIT_T, + AT_PARSER_WAIT_TERM, + AT_PARSER_SKIP_LINE, + } pstate; +}; + +static int wwan_hwsim_port_start(struct wwan_port *wport) +{ + struct wwan_hwsim_port *port = wwan_port_get_drvdata(wport); + + port->pstate = AT_PARSER_WAIT_A; + + return 0; +} + +static void wwan_hwsim_port_stop(struct wwan_port *wport) +{ +} + +/* Implements a minimalistic AT commands parser that echo input back and + * reply with 'OK' to each input command. See AT command protocol details in the + * ITU-T V.250 recomendations document. + * + * Be aware that this processor is not fully V.250 compliant. + */ +static int wwan_hwsim_port_tx(struct wwan_port *wport, struct sk_buff *in) +{ + struct wwan_hwsim_port *port = wwan_port_get_drvdata(wport); + struct sk_buff *out; + int i, n, s; + + /* Estimate a max possible number of commands by counting the number of + * termination chars (S3 param, CR by default). And then allocate the + * output buffer that will be enough to fit the echo and result codes of + * all commands. + */ + for (i = 0, n = 0; i < in->len; ++i) + if (in->data[i] == '\r') + n++; + n = in->len + n * (2 + 2 + 2); /* Output buffer size */ + out = alloc_skb(n, GFP_KERNEL); + if (!out) + return -ENOMEM; + + for (i = 0, s = 0; i < in->len; ++i) { + char c = in->data[i]; + + if (port->pstate == AT_PARSER_WAIT_A) { + if (c == 'A' || c == 'a') + port->pstate = AT_PARSER_WAIT_T; + else if (c != '\n') /* Ignore formating char */ + port->pstate = AT_PARSER_SKIP_LINE; + } else if (port->pstate == AT_PARSER_WAIT_T) { + if (c == 'T' || c == 't') + port->pstate = AT_PARSER_WAIT_TERM; + else + port->pstate = AT_PARSER_SKIP_LINE; + } else if (port->pstate == AT_PARSER_WAIT_TERM) { + if (c != '\r') + continue; + /* Consume the trailing formatting char as well */ + if ((i + 1) < in->len && in->data[i + 1] == '\n') + i++; + n = i - s + 1; + memcpy(skb_put(out, n), &in->data[s], n);/* Echo */ + memcpy(skb_put(out, 6), "\r\nOK\r\n", 6); + s = i + 1; + port->pstate = AT_PARSER_WAIT_A; + } else if (port->pstate == AT_PARSER_SKIP_LINE) { + if (c != '\r') + continue; + port->pstate = AT_PARSER_WAIT_A; + } + } + + if (i > s) { + /* Echo the processed portion of a not yet completed command */ + n = i - s; + memcpy(skb_put(out, n), &in->data[s], n); + } + + consume_skb(in); + + wwan_port_rx(wport, out); + + return 0; +} + +static const struct wwan_port_ops wwan_hwsim_port_ops = { + .start = wwan_hwsim_port_start, + .stop = wwan_hwsim_port_stop, + .tx = wwan_hwsim_port_tx, +}; + +static struct wwan_hwsim_port *wwan_hwsim_port_new(struct wwan_hwsim_dev *dev) +{ + struct wwan_hwsim_port *port; + int err; + + port = kzalloc(sizeof(*port), GFP_KERNEL); + if (!port) + return ERR_PTR(-ENOMEM); + + port->dev = dev; + + spin_lock(&dev->ports_lock); + port->id = dev->port_idx++; + spin_unlock(&dev->ports_lock); + + port->wwan = wwan_create_port(&dev->dev, WWAN_PORT_AT, + &wwan_hwsim_port_ops, + port); + if (IS_ERR(port->wwan)) { + err = PTR_ERR(port->wwan); + goto err_free_port; + } + + return port; + +err_free_port: + kfree(port); + + return ERR_PTR(err); +} + +static void wwan_hwsim_port_del(struct wwan_hwsim_port *port) +{ + wwan_remove_port(port->wwan); + kfree(port); +} + +static void wwan_hwsim_dev_release(struct device *sysdev) +{ + struct wwan_hwsim_dev *dev = container_of(sysdev, typeof(*dev), dev); + + kfree(dev); +} + +static struct wwan_hwsim_dev *wwan_hwsim_dev_new(void) +{ + struct wwan_hwsim_dev *dev; + int err; + + dev = kzalloc(sizeof(*dev), GFP_KERNEL); + if (!dev) + return ERR_PTR(-ENOMEM); + + spin_lock(&wwan_hwsim_devs_lock); + dev->id = wwan_hwsim_dev_idx++; + spin_unlock(&wwan_hwsim_devs_lock); + + dev->dev.release = wwan_hwsim_dev_release; + dev->dev.class = wwan_hwsim_class; + dev_set_name(&dev->dev, "hwsim%u", dev->id); + + spin_lock_init(&dev->ports_lock); + INIT_LIST_HEAD(&dev->ports); + + err = device_register(&dev->dev); + if (err) + goto err_free_dev; + + return dev; + +err_free_dev: + kfree(dev); + + return ERR_PTR(err); +} + +static void wwan_hwsim_dev_del(struct wwan_hwsim_dev *dev) +{ + spin_lock(&dev->ports_lock); + while (!list_empty(&dev->ports)) { + struct wwan_hwsim_port *port; + + port = list_first_entry(&dev->ports, struct wwan_hwsim_port, + list); + list_del(&port->list); + spin_unlock(&dev->ports_lock); + wwan_hwsim_port_del(port); + spin_lock(&dev->ports_lock); + } + spin_unlock(&dev->ports_lock); + + device_unregister(&dev->dev); + /* Memory will be freed in the device release callback */ +} + +static int __init wwan_hwsim_init_devs(void) +{ + struct wwan_hwsim_dev *dev; + int i, j; + + for (i = 0; i < wwan_hwsim_devsnum; ++i) { + dev = wwan_hwsim_dev_new(); + if (IS_ERR(dev)) + return PTR_ERR(dev); + + spin_lock(&wwan_hwsim_devs_lock); + list_add_tail(&dev->list, &wwan_hwsim_devs); + spin_unlock(&wwan_hwsim_devs_lock); + + /* Create a couple of ports per each device to accelerate + * the simulator readiness time. + */ + for (j = 0; j < 2; ++j) { + struct wwan_hwsim_port *port; + + port = wwan_hwsim_port_new(dev); + if (IS_ERR(port)) + return PTR_ERR(port); + + spin_lock(&dev->ports_lock); + list_add_tail(&port->list, &dev->ports); + spin_unlock(&dev->ports_lock); + } + } + + return 0; +} + +static void wwan_hwsim_free_devs(void) +{ + struct wwan_hwsim_dev *dev; + + spin_lock(&wwan_hwsim_devs_lock); + while (!list_empty(&wwan_hwsim_devs)) { + dev = list_first_entry(&wwan_hwsim_devs, struct wwan_hwsim_dev, + list); + list_del(&dev->list); + spin_unlock(&wwan_hwsim_devs_lock); + wwan_hwsim_dev_del(dev); + spin_lock(&wwan_hwsim_devs_lock); + } + spin_unlock(&wwan_hwsim_devs_lock); +} + +static int __init wwan_hwsim_init(void) +{ + int err; + + if (wwan_hwsim_devsnum < 0 || wwan_hwsim_devsnum > 128) + return -EINVAL; + + wwan_hwsim_class = class_create(THIS_MODULE, "wwan_hwsim"); + if (IS_ERR(wwan_hwsim_class)) + return PTR_ERR(wwan_hwsim_class); + + err = wwan_hwsim_init_devs(); + if (err) + goto err_clean_devs; + + return 0; + +err_clean_devs: + wwan_hwsim_free_devs(); + class_destroy(wwan_hwsim_class); + + return err; +} + +static void __exit wwan_hwsim_exit(void) +{ + wwan_hwsim_free_devs(); + class_destroy(wwan_hwsim_class); +} + +module_init(wwan_hwsim_init); +module_exit(wwan_hwsim_exit); + +MODULE_AUTHOR("Sergey Ryazanov"); +MODULE_DESCRIPTION("Device simulator for WWAN framework"); +MODULE_LICENSE("GPL"); -- 2.39.5