wwan_core.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/* Copyright (c) 2021, Linaro Ltd <loic.poulain@linaro.org> */
   3
   4#include <linux/bitmap.h>
   5#include <linux/err.h>
   6#include <linux/errno.h>
   7#include <linux/debugfs.h>
   8#include <linux/fs.h>
   9#include <linux/init.h>
  10#include <linux/idr.h>
  11#include <linux/kernel.h>
  12#include <linux/module.h>
  13#include <linux/poll.h>
  14#include <linux/skbuff.h>
  15#include <linux/slab.h>
  16#include <linux/types.h>
  17#include <linux/uaccess.h>
  18#include <linux/termios.h>
  19#include <linux/wwan.h>
  20#include <net/rtnetlink.h>
  21#include <uapi/linux/wwan.h>
  22
  23/* Maximum number of minors in use */
  24#define WWAN_MAX_MINORS		(1 << MINORBITS)
  25
  26static DEFINE_MUTEX(wwan_register_lock); /* WWAN device create|remove lock */
  27static DEFINE_IDA(minors); /* minors for WWAN port chardevs */
  28static DEFINE_IDA(wwan_dev_ids); /* for unique WWAN device IDs */
  29static const struct class wwan_class = {
  30	.name = "wwan",
  31};
  32static int wwan_major;
  33static struct dentry *wwan_debugfs_dir;
  34
  35#define to_wwan_dev(d) container_of(d, struct wwan_device, dev)
  36#define to_wwan_port(d) container_of(d, struct wwan_port, dev)
  37
  38/* WWAN port flags */
  39#define WWAN_PORT_TX_OFF	0
  40
  41/**
  42 * struct wwan_device - The structure that defines a WWAN device
  43 *
  44 * @id: WWAN device unique ID.
  45 * @dev: Underlying device.
  46 * @port_id: Current available port ID to pick.
  47 * @ops: wwan device ops
  48 * @ops_ctxt: context to pass to ops
  49 * @debugfs_dir:  WWAN device debugfs dir
  50 */
  51struct wwan_device {
  52	unsigned int id;
  53	struct device dev;
  54	atomic_t port_id;
  55	const struct wwan_ops *ops;
  56	void *ops_ctxt;
  57#ifdef CONFIG_WWAN_DEBUGFS
  58	struct dentry *debugfs_dir;
  59#endif
  60};
  61
  62/**
  63 * struct wwan_port - The structure that defines a WWAN port
  64 * @type: Port type
  65 * @start_count: Port start counter
  66 * @flags: Store port state and capabilities
  67 * @ops: Pointer to WWAN port operations
  68 * @ops_lock: Protect port ops
  69 * @dev: Underlying device
  70 * @rxq: Buffer inbound queue
  71 * @waitqueue: The waitqueue for port fops (read/write/poll)
  72 * @data_lock: Port specific data access serialization
  73 * @headroom_len: SKB reserved headroom size
  74 * @frag_len: Length to fragment packet
  75 * @at_data: AT port specific data
  76 */
  77struct wwan_port {
  78	enum wwan_port_type type;
  79	unsigned int start_count;
  80	unsigned long flags;
  81	const struct wwan_port_ops *ops;
  82	struct mutex ops_lock; /* Serialize ops + protect against removal */
  83	struct device dev;
  84	struct sk_buff_head rxq;
  85	wait_queue_head_t waitqueue;
  86	struct mutex data_lock;	/* Port specific data access serialization */
  87	size_t headroom_len;
  88	size_t frag_len;
  89	union {
  90		struct {
  91			struct ktermios termios;
  92			int mdmbits;
  93		} at_data;
  94	};
  95};
  96
  97static ssize_t index_show(struct device *dev, struct device_attribute *attr, char *buf)
  98{
  99	struct wwan_device *wwan = to_wwan_dev(dev);
 100
 101	return sprintf(buf, "%d\n", wwan->id);
 102}
 103static DEVICE_ATTR_RO(index);
 104
 105static struct attribute *wwan_dev_attrs[] = {
 106	&dev_attr_index.attr,
 107	NULL,
 108};
 109ATTRIBUTE_GROUPS(wwan_dev);
 110
 111static void wwan_dev_destroy(struct device *dev)
 112{
 113	struct wwan_device *wwandev = to_wwan_dev(dev);
 114
 115	ida_free(&wwan_dev_ids, wwandev->id);
 116	kfree(wwandev);
 117}
 118
 119static const struct device_type wwan_dev_type = {
 120	.name    = "wwan_dev",
 121	.release = wwan_dev_destroy,
 122	.groups = wwan_dev_groups,
 123};
 124
 125static int wwan_dev_parent_match(struct device *dev, const void *parent)
 126{
 127	return (dev->type == &wwan_dev_type &&
 128		(dev->parent == parent || dev == parent));
 129}
 130
 131static struct wwan_device *wwan_dev_get_by_parent(struct device *parent)
 132{
 133	struct device *dev;
 134
 135	dev = class_find_device(&wwan_class, NULL, parent, wwan_dev_parent_match);
 136	if (!dev)
 137		return ERR_PTR(-ENODEV);
 138
 139	return to_wwan_dev(dev);
 140}
 141
 142static int wwan_dev_name_match(struct device *dev, const void *name)
 143{
 144	return dev->type == &wwan_dev_type &&
 145	       strcmp(dev_name(dev), name) == 0;
 146}
 147
 148static struct wwan_device *wwan_dev_get_by_name(const char *name)
 149{
 150	struct device *dev;
 151
 152	dev = class_find_device(&wwan_class, NULL, name, wwan_dev_name_match);
 153	if (!dev)
 154		return ERR_PTR(-ENODEV);
 155
 156	return to_wwan_dev(dev);
 157}
 158
 159#ifdef CONFIG_WWAN_DEBUGFS
 160struct dentry *wwan_get_debugfs_dir(struct device *parent)
 161{
 162	struct wwan_device *wwandev;
 163
 164	wwandev = wwan_dev_get_by_parent(parent);
 165	if (IS_ERR(wwandev))
 166		return ERR_CAST(wwandev);
 167
 168	return wwandev->debugfs_dir;
 169}
 170EXPORT_SYMBOL_GPL(wwan_get_debugfs_dir);
 171
 172static int wwan_dev_debugfs_match(struct device *dev, const void *dir)
 173{
 174	struct wwan_device *wwandev;
 175
 176	if (dev->type != &wwan_dev_type)
 177		return 0;
 178
 179	wwandev = to_wwan_dev(dev);
 180
 181	return wwandev->debugfs_dir == dir;
 182}
 183
 184static struct wwan_device *wwan_dev_get_by_debugfs(struct dentry *dir)
 185{
 186	struct device *dev;
 187
 188	dev = class_find_device(&wwan_class, NULL, dir, wwan_dev_debugfs_match);
 189	if (!dev)
 190		return ERR_PTR(-ENODEV);
 191
 192	return to_wwan_dev(dev);
 193}
 194
 195void wwan_put_debugfs_dir(struct dentry *dir)
 196{
 197	struct wwan_device *wwandev = wwan_dev_get_by_debugfs(dir);
 198
 199	if (WARN_ON(IS_ERR(wwandev)))
 200		return;
 201
 202	/* wwan_dev_get_by_debugfs() also got a reference */
 203	put_device(&wwandev->dev);
 204	put_device(&wwandev->dev);
 205}
 206EXPORT_SYMBOL_GPL(wwan_put_debugfs_dir);
 207#endif
 208
 209/* This function allocates and registers a new WWAN device OR if a WWAN device
 210 * already exist for the given parent, it gets a reference and return it.
 211 * This function is not exported (for now), it is called indirectly via
 212 * wwan_create_port().
 213 */
 214static struct wwan_device *wwan_create_dev(struct device *parent)
 215{
 216	struct wwan_device *wwandev;
 217	int err, id;
 218
 219	/* The 'find-alloc-register' operation must be protected against
 220	 * concurrent execution, a WWAN device is possibly shared between
 221	 * multiple callers or concurrently unregistered from wwan_remove_dev().
 222	 */
 223	mutex_lock(&wwan_register_lock);
 224
 225	/* If wwandev already exists, return it */
 226	wwandev = wwan_dev_get_by_parent(parent);
 227	if (!IS_ERR(wwandev))
 228		goto done_unlock;
 229
 230	id = ida_alloc(&wwan_dev_ids, GFP_KERNEL);
 231	if (id < 0) {
 232		wwandev = ERR_PTR(id);
 233		goto done_unlock;
 234	}
 235
 236	wwandev = kzalloc(sizeof(*wwandev), GFP_KERNEL);
 237	if (!wwandev) {
 238		wwandev = ERR_PTR(-ENOMEM);
 239		ida_free(&wwan_dev_ids, id);
 240		goto done_unlock;
 241	}
 242
 243	wwandev->dev.parent = parent;
 244	wwandev->dev.class = &wwan_class;
 245	wwandev->dev.type = &wwan_dev_type;
 246	wwandev->id = id;
 247	dev_set_name(&wwandev->dev, "wwan%d", wwandev->id);
 248
 249	err = device_register(&wwandev->dev);
 250	if (err) {
 251		put_device(&wwandev->dev);
 252		wwandev = ERR_PTR(err);
 253		goto done_unlock;
 254	}
 255
 256#ifdef CONFIG_WWAN_DEBUGFS
 257	wwandev->debugfs_dir =
 258			debugfs_create_dir(kobject_name(&wwandev->dev.kobj),
 259					   wwan_debugfs_dir);
 260#endif
 261
 262done_unlock:
 263	mutex_unlock(&wwan_register_lock);
 264
 265	return wwandev;
 266}
 267
 268static int is_wwan_child(struct device *dev, void *data)
 269{
 270	return dev->class == &wwan_class;
 271}
 272
 273static void wwan_remove_dev(struct wwan_device *wwandev)
 274{
 275	int ret;
 276
 277	/* Prevent concurrent picking from wwan_create_dev */
 278	mutex_lock(&wwan_register_lock);
 279
 280	/* WWAN device is created and registered (get+add) along with its first
 281	 * child port, and subsequent port registrations only grab a reference
 282	 * (get). The WWAN device must then be unregistered (del+put) along with
 283	 * its last port, and reference simply dropped (put) otherwise. In the
 284	 * same fashion, we must not unregister it when the ops are still there.
 285	 */
 286	if (wwandev->ops)
 287		ret = 1;
 288	else
 289		ret = device_for_each_child(&wwandev->dev, NULL, is_wwan_child);
 290
 291	if (!ret) {
 292#ifdef CONFIG_WWAN_DEBUGFS
 293		debugfs_remove_recursive(wwandev->debugfs_dir);
 294#endif
 295		device_unregister(&wwandev->dev);
 296	} else {
 297		put_device(&wwandev->dev);
 298	}
 299
 300	mutex_unlock(&wwan_register_lock);
 301}
 302
 303/* ------- WWAN port management ------- */
 304
 305static const struct {
 306	const char * const name;	/* Port type name */
 307	const char * const devsuf;	/* Port device name suffix */
 308} wwan_port_types[WWAN_PORT_MAX + 1] = {
 309	[WWAN_PORT_AT] = {
 310		.name = "AT",
 311		.devsuf = "at",
 312	},
 313	[WWAN_PORT_MBIM] = {
 314		.name = "MBIM",
 315		.devsuf = "mbim",
 316	},
 317	[WWAN_PORT_QMI] = {
 318		.name = "QMI",
 319		.devsuf = "qmi",
 320	},
 321	[WWAN_PORT_QCDM] = {
 322		.name = "QCDM",
 323		.devsuf = "qcdm",
 324	},
 325	[WWAN_PORT_FIREHOSE] = {
 326		.name = "FIREHOSE",
 327		.devsuf = "firehose",
 328	},
 329	[WWAN_PORT_XMMRPC] = {
 330		.name = "XMMRPC",
 331		.devsuf = "xmmrpc",
 332	},
 333	[WWAN_PORT_FASTBOOT] = {
 334		.name = "FASTBOOT",
 335		.devsuf = "fastboot",
 336	},
 337	[WWAN_PORT_ADB] = {
 338		.name = "ADB",
 339		.devsuf = "adb",
 340	},
 341	[WWAN_PORT_MIPC] = {
 342		.name = "MIPC",
 343		.devsuf = "mipc",
 344	},
 345};
 346
 347static ssize_t type_show(struct device *dev, struct device_attribute *attr,
 348			 char *buf)
 349{
 350	struct wwan_port *port = to_wwan_port(dev);
 351
 352	return sprintf(buf, "%s\n", wwan_port_types[port->type].name);
 353}
 354static DEVICE_ATTR_RO(type);
 355
 356static struct attribute *wwan_port_attrs[] = {
 357	&dev_attr_type.attr,
 358	NULL,
 359};
 360ATTRIBUTE_GROUPS(wwan_port);
 361
 362static void wwan_port_destroy(struct device *dev)
 363{
 364	struct wwan_port *port = to_wwan_port(dev);
 365
 366	ida_free(&minors, MINOR(port->dev.devt));
 367	mutex_destroy(&port->data_lock);
 368	mutex_destroy(&port->ops_lock);
 369	kfree(port);
 370}
 371
 372static const struct device_type wwan_port_dev_type = {
 373	.name = "wwan_port",
 374	.release = wwan_port_destroy,
 375	.groups = wwan_port_groups,
 376};
 377
 378static int wwan_port_minor_match(struct device *dev, const void *minor)
 379{
 380	return (dev->type == &wwan_port_dev_type &&
 381		MINOR(dev->devt) == *(unsigned int *)minor);
 382}
 383
 384static struct wwan_port *wwan_port_get_by_minor(unsigned int minor)
 385{
 386	struct device *dev;
 387
 388	dev = class_find_device(&wwan_class, NULL, &minor, wwan_port_minor_match);
 389	if (!dev)
 390		return ERR_PTR(-ENODEV);
 391
 392	return to_wwan_port(dev);
 393}
 394
 395/* Allocate and set unique name based on passed format
 396 *
 397 * Name allocation approach is highly inspired by the __dev_alloc_name()
 398 * function.
 399 *
 400 * To avoid names collision, the caller must prevent the new port device
 401 * registration as well as concurrent invocation of this function.
 402 */
 403static int __wwan_port_dev_assign_name(struct wwan_port *port, const char *fmt)
 404{
 405	struct wwan_device *wwandev = to_wwan_dev(port->dev.parent);
 406	const unsigned int max_ports = PAGE_SIZE * 8;
 407	struct class_dev_iter iter;
 408	unsigned long *idmap;
 409	struct device *dev;
 410	char buf[0x20];
 411	int id;
 412
 413	idmap = bitmap_zalloc(max_ports, GFP_KERNEL);
 414	if (!idmap)
 415		return -ENOMEM;
 416
 417	/* Collect ids of same name format ports */
 418	class_dev_iter_init(&iter, &wwan_class, NULL, &wwan_port_dev_type);
 419	while ((dev = class_dev_iter_next(&iter))) {
 420		if (dev->parent != &wwandev->dev)
 421			continue;
 422		if (sscanf(dev_name(dev), fmt, &id) != 1)
 423			continue;
 424		if (id < 0 || id >= max_ports)
 425			continue;
 426		set_bit(id, idmap);
 427	}
 428	class_dev_iter_exit(&iter);
 429
 430	/* Allocate unique id */
 431	id = find_first_zero_bit(idmap, max_ports);
 432	bitmap_free(idmap);
 433
 434	snprintf(buf, sizeof(buf), fmt, id);	/* Name generation */
 435
 436	dev = device_find_child_by_name(&wwandev->dev, buf);
 437	if (dev) {
 438		put_device(dev);
 439		return -ENFILE;
 440	}
 441
 442	return dev_set_name(&port->dev, "%s", buf);
 443}
 444
 445struct wwan_port *wwan_create_port(struct device *parent,
 446				   enum wwan_port_type type,
 447				   const struct wwan_port_ops *ops,
 448				   struct wwan_port_caps *caps,
 449				   void *drvdata)
 450{
 451	struct wwan_device *wwandev;
 452	struct wwan_port *port;
 453	char namefmt[0x20];
 454	int minor, err;
 455
 456	if (type > WWAN_PORT_MAX || !ops)
 457		return ERR_PTR(-EINVAL);
 458
 459	/* A port is always a child of a WWAN device, retrieve (allocate or
 460	 * pick) the WWAN device based on the provided parent device.
 461	 */
 462	wwandev = wwan_create_dev(parent);
 463	if (IS_ERR(wwandev))
 464		return ERR_CAST(wwandev);
 465
 466	/* A port is exposed as character device, get a minor */
 467	minor = ida_alloc_range(&minors, 0, WWAN_MAX_MINORS - 1, GFP_KERNEL);
 468	if (minor < 0) {
 469		err = minor;
 470		goto error_wwandev_remove;
 471	}
 472
 473	port = kzalloc(sizeof(*port), GFP_KERNEL);
 474	if (!port) {
 475		err = -ENOMEM;
 476		ida_free(&minors, minor);
 477		goto error_wwandev_remove;
 478	}
 479
 480	port->type = type;
 481	port->ops = ops;
 482	port->frag_len = caps ? caps->frag_len : SIZE_MAX;
 483	port->headroom_len = caps ? caps->headroom_len : 0;
 484	mutex_init(&port->ops_lock);
 485	skb_queue_head_init(&port->rxq);
 486	init_waitqueue_head(&port->waitqueue);
 487	mutex_init(&port->data_lock);
 488
 489	port->dev.parent = &wwandev->dev;
 490	port->dev.class = &wwan_class;
 491	port->dev.type = &wwan_port_dev_type;
 492	port->dev.devt = MKDEV(wwan_major, minor);
 493	dev_set_drvdata(&port->dev, drvdata);
 494
 495	/* allocate unique name based on wwan device id, port type and number */
 496	snprintf(namefmt, sizeof(namefmt), "wwan%u%s%%d", wwandev->id,
 497		 wwan_port_types[port->type].devsuf);
 498
 499	/* Serialize ports registration */
 500	mutex_lock(&wwan_register_lock);
 501
 502	__wwan_port_dev_assign_name(port, namefmt);
 503	err = device_register(&port->dev);
 504
 505	mutex_unlock(&wwan_register_lock);
 506
 507	if (err)
 508		goto error_put_device;
 509
 510	dev_info(&wwandev->dev, "port %s attached\n", dev_name(&port->dev));
 511	return port;
 512
 513error_put_device:
 514	put_device(&port->dev);
 515error_wwandev_remove:
 516	wwan_remove_dev(wwandev);
 517
 518	return ERR_PTR(err);
 519}
 520EXPORT_SYMBOL_GPL(wwan_create_port);
 521
 522void wwan_remove_port(struct wwan_port *port)
 523{
 524	struct wwan_device *wwandev = to_wwan_dev(port->dev.parent);
 525
 526	mutex_lock(&port->ops_lock);
 527	if (port->start_count)
 528		port->ops->stop(port);
 529	port->ops = NULL; /* Prevent any new port operations (e.g. from fops) */
 530	mutex_unlock(&port->ops_lock);
 531
 532	wake_up_interruptible(&port->waitqueue);
 533
 534	skb_queue_purge(&port->rxq);
 535	dev_set_drvdata(&port->dev, NULL);
 536
 537	dev_info(&wwandev->dev, "port %s disconnected\n", dev_name(&port->dev));
 538	device_unregister(&port->dev);
 539
 540	/* Release related wwan device */
 541	wwan_remove_dev(wwandev);
 542}
 543EXPORT_SYMBOL_GPL(wwan_remove_port);
 544
 545void wwan_port_rx(struct wwan_port *port, struct sk_buff *skb)
 546{
 547	skb_queue_tail(&port->rxq, skb);
 548	wake_up_interruptible(&port->waitqueue);
 549}
 550EXPORT_SYMBOL_GPL(wwan_port_rx);
 551
 552void wwan_port_txon(struct wwan_port *port)
 553{
 554	clear_bit(WWAN_PORT_TX_OFF, &port->flags);
 555	wake_up_interruptible(&port->waitqueue);
 556}
 557EXPORT_SYMBOL_GPL(wwan_port_txon);
 558
 559void wwan_port_txoff(struct wwan_port *port)
 560{
 561	set_bit(WWAN_PORT_TX_OFF, &port->flags);
 562}
 563EXPORT_SYMBOL_GPL(wwan_port_txoff);
 564
 565void *wwan_port_get_drvdata(struct wwan_port *port)
 566{
 567	return dev_get_drvdata(&port->dev);
 568}
 569EXPORT_SYMBOL_GPL(wwan_port_get_drvdata);
 570
 571static int wwan_port_op_start(struct wwan_port *port)
 572{
 573	int ret = 0;
 574
 575	mutex_lock(&port->ops_lock);
 576	if (!port->ops) { /* Port got unplugged */
 577		ret = -ENODEV;
 578		goto out_unlock;
 579	}
 580
 581	/* If port is already started, don't start again */
 582	if (!port->start_count)
 583		ret = port->ops->start(port);
 584
 585	if (!ret)
 586		port->start_count++;
 587
 588out_unlock:
 589	mutex_unlock(&port->ops_lock);
 590
 591	return ret;
 592}
 593
 594static void wwan_port_op_stop(struct wwan_port *port)
 595{
 596	mutex_lock(&port->ops_lock);
 597	port->start_count--;
 598	if (!port->start_count) {
 599		if (port->ops)
 600			port->ops->stop(port);
 601		skb_queue_purge(&port->rxq);
 602	}
 603	mutex_unlock(&port->ops_lock);
 604}
 605
 606static int wwan_port_op_tx(struct wwan_port *port, struct sk_buff *skb,
 607			   bool nonblock)
 608{
 609	int ret;
 610
 611	mutex_lock(&port->ops_lock);
 612	if (!port->ops) { /* Port got unplugged */
 613		ret = -ENODEV;
 614		goto out_unlock;
 615	}
 616
 617	if (nonblock || !port->ops->tx_blocking)
 618		ret = port->ops->tx(port, skb);
 619	else
 620		ret = port->ops->tx_blocking(port, skb);
 621
 622out_unlock:
 623	mutex_unlock(&port->ops_lock);
 624
 625	return ret;
 626}
 627
 628static bool is_read_blocked(struct wwan_port *port)
 629{
 630	return skb_queue_empty(&port->rxq) && port->ops;
 631}
 632
 633static bool is_write_blocked(struct wwan_port *port)
 634{
 635	return test_bit(WWAN_PORT_TX_OFF, &port->flags) && port->ops;
 636}
 637
 638static int wwan_wait_rx(struct wwan_port *port, bool nonblock)
 639{
 640	if (!is_read_blocked(port))
 641		return 0;
 642
 643	if (nonblock)
 644		return -EAGAIN;
 645
 646	if (wait_event_interruptible(port->waitqueue, !is_read_blocked(port)))
 647		return -ERESTARTSYS;
 648
 649	return 0;
 650}
 651
 652static int wwan_wait_tx(struct wwan_port *port, bool nonblock)
 653{
 654	if (!is_write_blocked(port))
 655		return 0;
 656
 657	if (nonblock)
 658		return -EAGAIN;
 659
 660	if (wait_event_interruptible(port->waitqueue, !is_write_blocked(port)))
 661		return -ERESTARTSYS;
 662
 663	return 0;
 664}
 665
 666static int wwan_port_fops_open(struct inode *inode, struct file *file)
 667{
 668	struct wwan_port *port;
 669	int err = 0;
 670
 671	port = wwan_port_get_by_minor(iminor(inode));
 672	if (IS_ERR(port))
 673		return PTR_ERR(port);
 674
 675	file->private_data = port;
 676	stream_open(inode, file);
 677
 678	err = wwan_port_op_start(port);
 679	if (err)
 680		put_device(&port->dev);
 681
 682	return err;
 683}
 684
 685static int wwan_port_fops_release(struct inode *inode, struct file *filp)
 686{
 687	struct wwan_port *port = filp->private_data;
 688
 689	wwan_port_op_stop(port);
 690	put_device(&port->dev);
 691
 692	return 0;
 693}
 694
 695static ssize_t wwan_port_fops_read(struct file *filp, char __user *buf,
 696				   size_t count, loff_t *ppos)
 697{
 698	struct wwan_port *port = filp->private_data;
 699	struct sk_buff *skb;
 700	size_t copied;
 701	int ret;
 702
 703	ret = wwan_wait_rx(port, !!(filp->f_flags & O_NONBLOCK));
 704	if (ret)
 705		return ret;
 706
 707	skb = skb_dequeue(&port->rxq);
 708	if (!skb)
 709		return -EIO;
 710
 711	copied = min_t(size_t, count, skb->len);
 712	if (copy_to_user(buf, skb->data, copied)) {
 713		kfree_skb(skb);
 714		return -EFAULT;
 715	}
 716	skb_pull(skb, copied);
 717
 718	/* skb is not fully consumed, keep it in the queue */
 719	if (skb->len)
 720		skb_queue_head(&port->rxq, skb);
 721	else
 722		consume_skb(skb);
 723
 724	return copied;
 725}
 726
 727static ssize_t wwan_port_fops_write(struct file *filp, const char __user *buf,
 728				    size_t count, loff_t *offp)
 729{
 730	struct sk_buff *skb, *head = NULL, *tail = NULL;
 731	struct wwan_port *port = filp->private_data;
 732	size_t frag_len, remain = count;
 733	int ret;
 734
 735	ret = wwan_wait_tx(port, !!(filp->f_flags & O_NONBLOCK));
 736	if (ret)
 737		return ret;
 738
 739	do {
 740		frag_len = min(remain, port->frag_len);
 741		skb = alloc_skb(frag_len + port->headroom_len, GFP_KERNEL);
 742		if (!skb) {
 743			ret = -ENOMEM;
 744			goto freeskb;
 745		}
 746		skb_reserve(skb, port->headroom_len);
 747
 748		if (!head) {
 749			head = skb;
 750		} else if (!tail) {
 751			skb_shinfo(head)->frag_list = skb;
 752			tail = skb;
 753		} else {
 754			tail->next = skb;
 755			tail = skb;
 756		}
 757
 758		if (copy_from_user(skb_put(skb, frag_len), buf + count - remain, frag_len)) {
 759			ret = -EFAULT;
 760			goto freeskb;
 761		}
 762
 763		if (skb != head) {
 764			head->data_len += skb->len;
 765			head->len += skb->len;
 766			head->truesize += skb->truesize;
 767		}
 768	} while (remain -= frag_len);
 769
 770	ret = wwan_port_op_tx(port, head, !!(filp->f_flags & O_NONBLOCK));
 771	if (!ret)
 772		return count;
 773
 774freeskb:
 775	kfree_skb(head);
 776	return ret;
 777}
 778
 779static __poll_t wwan_port_fops_poll(struct file *filp, poll_table *wait)
 780{
 781	struct wwan_port *port = filp->private_data;
 782	__poll_t mask = 0;
 783
 784	poll_wait(filp, &port->waitqueue, wait);
 785
 786	mutex_lock(&port->ops_lock);
 787	if (port->ops && port->ops->tx_poll)
 788		mask |= port->ops->tx_poll(port, filp, wait);
 789	else if (!is_write_blocked(port))
 790		mask |= EPOLLOUT | EPOLLWRNORM;
 791	if (!is_read_blocked(port))
 792		mask |= EPOLLIN | EPOLLRDNORM;
 793	if (!port->ops)
 794		mask |= EPOLLHUP | EPOLLERR;
 795	mutex_unlock(&port->ops_lock);
 796
 797	return mask;
 798}
 799
 800/* Implements minimalistic stub terminal IOCTLs support */
 801static long wwan_port_fops_at_ioctl(struct wwan_port *port, unsigned int cmd,
 802				    unsigned long arg)
 803{
 804	int ret = 0;
 805
 806	mutex_lock(&port->data_lock);
 807
 808	switch (cmd) {
 809	case TCFLSH:
 810		break;
 811
 812	case TCGETS:
 813		if (copy_to_user((void __user *)arg, &port->at_data.termios,
 814				 sizeof(struct termios)))
 815			ret = -EFAULT;
 816		break;
 817
 818	case TCSETS:
 819	case TCSETSW:
 820	case TCSETSF:
 821		if (copy_from_user(&port->at_data.termios, (void __user *)arg,
 822				   sizeof(struct termios)))
 823			ret = -EFAULT;
 824		break;
 825
 826#ifdef TCGETS2
 827	case TCGETS2:
 828		if (copy_to_user((void __user *)arg, &port->at_data.termios,
 829				 sizeof(struct termios2)))
 830			ret = -EFAULT;
 831		break;
 832
 833	case TCSETS2:
 834	case TCSETSW2:
 835	case TCSETSF2:
 836		if (copy_from_user(&port->at_data.termios, (void __user *)arg,
 837				   sizeof(struct termios2)))
 838			ret = -EFAULT;
 839		break;
 840#endif
 841
 842	case TIOCMGET:
 843		ret = put_user(port->at_data.mdmbits, (int __user *)arg);
 844		break;
 845
 846	case TIOCMSET:
 847	case TIOCMBIC:
 848	case TIOCMBIS: {
 849		int mdmbits;
 850
 851		if (copy_from_user(&mdmbits, (int __user *)arg, sizeof(int))) {
 852			ret = -EFAULT;
 853			break;
 854		}
 855		if (cmd == TIOCMBIC)
 856			port->at_data.mdmbits &= ~mdmbits;
 857		else if (cmd == TIOCMBIS)
 858			port->at_data.mdmbits |= mdmbits;
 859		else
 860			port->at_data.mdmbits = mdmbits;
 861		break;
 862	}
 863
 864	default:
 865		ret = -ENOIOCTLCMD;
 866	}
 867
 868	mutex_unlock(&port->data_lock);
 869
 870	return ret;
 871}
 872
 873static long wwan_port_fops_ioctl(struct file *filp, unsigned int cmd,
 874				 unsigned long arg)
 875{
 876	struct wwan_port *port = filp->private_data;
 877	int res;
 878
 879	if (port->type == WWAN_PORT_AT) {	/* AT port specific IOCTLs */
 880		res = wwan_port_fops_at_ioctl(port, cmd, arg);
 881		if (res != -ENOIOCTLCMD)
 882			return res;
 883	}
 884
 885	switch (cmd) {
 886	case TIOCINQ: {	/* aka SIOCINQ aka FIONREAD */
 887		unsigned long flags;
 888		struct sk_buff *skb;
 889		int amount = 0;
 890
 891		spin_lock_irqsave(&port->rxq.lock, flags);
 892		skb_queue_walk(&port->rxq, skb)
 893			amount += skb->len;
 894		spin_unlock_irqrestore(&port->rxq.lock, flags);
 895
 896		return put_user(amount, (int __user *)arg);
 897	}
 898
 899	default:
 900		return -ENOIOCTLCMD;
 901	}
 902}
 903
 904static const struct file_operations wwan_port_fops = {
 905	.owner = THIS_MODULE,
 906	.open = wwan_port_fops_open,
 907	.release = wwan_port_fops_release,
 908	.read = wwan_port_fops_read,
 909	.write = wwan_port_fops_write,
 910	.poll = wwan_port_fops_poll,
 911	.unlocked_ioctl = wwan_port_fops_ioctl,
 912#ifdef CONFIG_COMPAT
 913	.compat_ioctl = compat_ptr_ioctl,
 914#endif
 915	.llseek = noop_llseek,
 916};
 917
 918static int wwan_rtnl_validate(struct nlattr *tb[], struct nlattr *data[],
 919			      struct netlink_ext_ack *extack)
 920{
 921	if (!data)
 922		return -EINVAL;
 923
 924	if (!tb[IFLA_PARENT_DEV_NAME])
 925		return -EINVAL;
 926
 927	if (!data[IFLA_WWAN_LINK_ID])
 928		return -EINVAL;
 929
 930	return 0;
 931}
 932
 933static const struct device_type wwan_type = { .name = "wwan" };
 934
 935static struct net_device *wwan_rtnl_alloc(struct nlattr *tb[],
 936					  const char *ifname,
 937					  unsigned char name_assign_type,
 938					  unsigned int num_tx_queues,
 939					  unsigned int num_rx_queues)
 940{
 941	const char *devname = nla_data(tb[IFLA_PARENT_DEV_NAME]);
 942	struct wwan_device *wwandev = wwan_dev_get_by_name(devname);
 943	struct net_device *dev;
 944	unsigned int priv_size;
 945
 946	if (IS_ERR(wwandev))
 947		return ERR_CAST(wwandev);
 948
 949	/* only supported if ops were registered (not just ports) */
 950	if (!wwandev->ops) {
 951		dev = ERR_PTR(-EOPNOTSUPP);
 952		goto out;
 953	}
 954
 955	priv_size = sizeof(struct wwan_netdev_priv) + wwandev->ops->priv_size;
 956	dev = alloc_netdev_mqs(priv_size, ifname, name_assign_type,
 957			       wwandev->ops->setup, num_tx_queues, num_rx_queues);
 958
 959	if (dev) {
 960		SET_NETDEV_DEV(dev, &wwandev->dev);
 961		SET_NETDEV_DEVTYPE(dev, &wwan_type);
 962	}
 963
 964out:
 965	/* release the reference */
 966	put_device(&wwandev->dev);
 967	return dev;
 968}
 969
 970static int wwan_rtnl_newlink(struct net_device *dev,
 971			     struct rtnl_newlink_params *params,
 972			     struct netlink_ext_ack *extack)
 973{
 974	struct wwan_device *wwandev = wwan_dev_get_by_parent(dev->dev.parent);
 975	struct wwan_netdev_priv *priv = netdev_priv(dev);
 976	struct nlattr **data = params->data;
 977	u32 link_id;
 978	int ret;
 979
 980	link_id = nla_get_u32(data[IFLA_WWAN_LINK_ID]);
 981
 982	if (IS_ERR(wwandev))
 983		return PTR_ERR(wwandev);
 984
 985	/* shouldn't have a netdev (left) with us as parent so WARN */
 986	if (WARN_ON(!wwandev->ops)) {
 987		ret = -EOPNOTSUPP;
 988		goto out;
 989	}
 990
 991	priv->link_id = link_id;
 992	if (wwandev->ops->newlink)
 993		ret = wwandev->ops->newlink(wwandev->ops_ctxt, dev,
 994					    link_id, extack);
 995	else
 996		ret = register_netdevice(dev);
 997
 998out:
 999	/* release the reference */
1000	put_device(&wwandev->dev);
1001	return ret;
1002}
1003
1004static void wwan_rtnl_dellink(struct net_device *dev, struct list_head *head)
1005{
1006	struct wwan_device *wwandev = wwan_dev_get_by_parent(dev->dev.parent);
1007
1008	if (IS_ERR(wwandev))
1009		return;
1010
1011	/* shouldn't have a netdev (left) with us as parent so WARN */
1012	if (WARN_ON(!wwandev->ops))
1013		goto out;
1014
1015	if (wwandev->ops->dellink)
1016		wwandev->ops->dellink(wwandev->ops_ctxt, dev, head);
1017	else
1018		unregister_netdevice_queue(dev, head);
1019
1020out:
1021	/* release the reference */
1022	put_device(&wwandev->dev);
1023}
1024
1025static size_t wwan_rtnl_get_size(const struct net_device *dev)
1026{
1027	return
1028		nla_total_size(4) +	/* IFLA_WWAN_LINK_ID */
1029		0;
1030}
1031
1032static int wwan_rtnl_fill_info(struct sk_buff *skb,
1033			       const struct net_device *dev)
1034{
1035	struct wwan_netdev_priv *priv = netdev_priv(dev);
1036
1037	if (nla_put_u32(skb, IFLA_WWAN_LINK_ID, priv->link_id))
1038		goto nla_put_failure;
1039
1040	return 0;
1041
1042nla_put_failure:
1043	return -EMSGSIZE;
1044}
1045
1046static const struct nla_policy wwan_rtnl_policy[IFLA_WWAN_MAX + 1] = {
1047	[IFLA_WWAN_LINK_ID] = { .type = NLA_U32 },
1048};
1049
1050static struct rtnl_link_ops wwan_rtnl_link_ops __read_mostly = {
1051	.kind = "wwan",
1052	.maxtype = IFLA_WWAN_MAX,
1053	.alloc = wwan_rtnl_alloc,
1054	.validate = wwan_rtnl_validate,
1055	.newlink = wwan_rtnl_newlink,
1056	.dellink = wwan_rtnl_dellink,
1057	.get_size = wwan_rtnl_get_size,
1058	.fill_info = wwan_rtnl_fill_info,
1059	.policy = wwan_rtnl_policy,
1060};
1061
1062static void wwan_create_default_link(struct wwan_device *wwandev,
1063				     u32 def_link_id)
1064{
1065	struct nlattr *tb[IFLA_MAX + 1], *linkinfo[IFLA_INFO_MAX + 1];
1066	struct nlattr *data[IFLA_WWAN_MAX + 1];
1067	struct rtnl_newlink_params params = {
1068		.src_net = &init_net,
1069		.tb = tb,
1070		.data = data,
1071	};
1072	struct net_device *dev;
1073	struct nlmsghdr *nlh;
1074	struct sk_buff *msg;
1075
1076	/* Forge attributes required to create a WWAN netdev. We first
1077	 * build a netlink message and then parse it. This looks
1078	 * odd, but such approach is less error prone.
1079	 */
1080	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1081	if (WARN_ON(!msg))
1082		return;
1083	nlh = nlmsg_put(msg, 0, 0, RTM_NEWLINK, 0, 0);
1084	if (WARN_ON(!nlh))
1085		goto free_attrs;
1086
1087	if (nla_put_string(msg, IFLA_PARENT_DEV_NAME, dev_name(&wwandev->dev)))
1088		goto free_attrs;
1089	tb[IFLA_LINKINFO] = nla_nest_start(msg, IFLA_LINKINFO);
1090	if (!tb[IFLA_LINKINFO])
1091		goto free_attrs;
1092	linkinfo[IFLA_INFO_DATA] = nla_nest_start(msg, IFLA_INFO_DATA);
1093	if (!linkinfo[IFLA_INFO_DATA])
1094		goto free_attrs;
1095	if (nla_put_u32(msg, IFLA_WWAN_LINK_ID, def_link_id))
1096		goto free_attrs;
1097	nla_nest_end(msg, linkinfo[IFLA_INFO_DATA]);
1098	nla_nest_end(msg, tb[IFLA_LINKINFO]);
1099
1100	nlmsg_end(msg, nlh);
1101
1102	/* The next three parsing calls can not fail */
1103	nlmsg_parse_deprecated(nlh, 0, tb, IFLA_MAX, NULL, NULL);
1104	nla_parse_nested_deprecated(linkinfo, IFLA_INFO_MAX, tb[IFLA_LINKINFO],
1105				    NULL, NULL);
1106	nla_parse_nested_deprecated(data, IFLA_WWAN_MAX,
1107				    linkinfo[IFLA_INFO_DATA], NULL, NULL);
1108
1109	rtnl_lock();
1110
1111	dev = rtnl_create_link(&init_net, "wwan%d", NET_NAME_ENUM,
1112			       &wwan_rtnl_link_ops, tb, NULL);
1113	if (WARN_ON(IS_ERR(dev)))
1114		goto unlock;
1115
1116	if (WARN_ON(wwan_rtnl_newlink(dev, &params, NULL))) {
1117		free_netdev(dev);
1118		goto unlock;
1119	}
1120
1121	rtnl_configure_link(dev, NULL, 0, NULL); /* Link initialized, notify new link */
1122
1123unlock:
1124	rtnl_unlock();
1125
1126free_attrs:
1127	nlmsg_free(msg);
1128}
1129
1130/**
1131 * wwan_register_ops - register WWAN device ops
1132 * @parent: Device to use as parent and shared by all WWAN ports and
1133 *	created netdevs
1134 * @ops: operations to register
1135 * @ctxt: context to pass to operations
1136 * @def_link_id: id of the default link that will be automatically created by
1137 *	the WWAN core for the WWAN device. The default link will not be created
1138 *	if the passed value is WWAN_NO_DEFAULT_LINK.
1139 *
1140 * Returns: 0 on success, a negative error code on failure
1141 */
1142int wwan_register_ops(struct device *parent, const struct wwan_ops *ops,
1143		      void *ctxt, u32 def_link_id)
1144{
1145	struct wwan_device *wwandev;
1146
1147	if (WARN_ON(!parent || !ops || !ops->setup))
1148		return -EINVAL;
1149
1150	wwandev = wwan_create_dev(parent);
1151	if (IS_ERR(wwandev))
1152		return PTR_ERR(wwandev);
1153
1154	if (WARN_ON(wwandev->ops)) {
1155		wwan_remove_dev(wwandev);
1156		return -EBUSY;
1157	}
1158
1159	wwandev->ops = ops;
1160	wwandev->ops_ctxt = ctxt;
1161
1162	/* NB: we do not abort ops registration in case of default link
1163	 * creation failure. Link ops is the management interface, while the
1164	 * default link creation is a service option. And we should not prevent
1165	 * a user from manually creating a link latter if service option failed
1166	 * now.
1167	 */
1168	if (def_link_id != WWAN_NO_DEFAULT_LINK)
1169		wwan_create_default_link(wwandev, def_link_id);
1170
1171	return 0;
1172}
1173EXPORT_SYMBOL_GPL(wwan_register_ops);
1174
1175/* Enqueue child netdev deletion */
1176static int wwan_child_dellink(struct device *dev, void *data)
1177{
1178	struct list_head *kill_list = data;
1179
1180	if (dev->type == &wwan_type)
1181		wwan_rtnl_dellink(to_net_dev(dev), kill_list);
1182
1183	return 0;
1184}
1185
1186/**
1187 * wwan_unregister_ops - remove WWAN device ops
1188 * @parent: Device to use as parent and shared by all WWAN ports and
1189 *	created netdevs
1190 */
1191void wwan_unregister_ops(struct device *parent)
1192{
1193	struct wwan_device *wwandev = wwan_dev_get_by_parent(parent);
1194	LIST_HEAD(kill_list);
1195
1196	if (WARN_ON(IS_ERR(wwandev)))
1197		return;
1198	if (WARN_ON(!wwandev->ops)) {
1199		put_device(&wwandev->dev);
1200		return;
1201	}
1202
1203	/* put the reference obtained by wwan_dev_get_by_parent(),
1204	 * we should still have one (that the owner is giving back
1205	 * now) due to the ops being assigned.
1206	 */
1207	put_device(&wwandev->dev);
1208
1209	rtnl_lock();	/* Prevent concurrent netdev(s) creation/destroying */
1210
1211	/* Remove all child netdev(s), using batch removing */
1212	device_for_each_child(&wwandev->dev, &kill_list,
1213			      wwan_child_dellink);
1214	unregister_netdevice_many(&kill_list);
1215
1216	wwandev->ops = NULL;	/* Finally remove ops */
1217
1218	rtnl_unlock();
1219
1220	wwandev->ops_ctxt = NULL;
1221	wwan_remove_dev(wwandev);
1222}
1223EXPORT_SYMBOL_GPL(wwan_unregister_ops);
1224
1225static int __init wwan_init(void)
1226{
1227	int err;
1228
1229	err = rtnl_link_register(&wwan_rtnl_link_ops);
1230	if (err)
1231		return err;
1232
1233	err = class_register(&wwan_class);
1234	if (err)
1235		goto unregister;
1236
1237	/* chrdev used for wwan ports */
1238	wwan_major = __register_chrdev(0, 0, WWAN_MAX_MINORS, "wwan_port",
1239				       &wwan_port_fops);
1240	if (wwan_major < 0) {
1241		err = wwan_major;
1242		goto destroy;
1243	}
1244
1245#ifdef CONFIG_WWAN_DEBUGFS
1246	wwan_debugfs_dir = debugfs_create_dir("wwan", NULL);
1247#endif
1248
1249	return 0;
1250
1251destroy:
1252	class_unregister(&wwan_class);
1253unregister:
1254	rtnl_link_unregister(&wwan_rtnl_link_ops);
1255	return err;
1256}
1257
1258static void __exit wwan_exit(void)
1259{
1260	debugfs_remove_recursive(wwan_debugfs_dir);
1261	__unregister_chrdev(wwan_major, 0, WWAN_MAX_MINORS, "wwan_port");
1262	rtnl_link_unregister(&wwan_rtnl_link_ops);
1263	class_unregister(&wwan_class);
1264}
1265
1266module_init(wwan_init);
1267module_exit(wwan_exit);
1268
1269MODULE_AUTHOR("Loic Poulain <loic.poulain@linaro.org>");
1270MODULE_DESCRIPTION("WWAN core");
1271MODULE_LICENSE("GPL v2");