drivers/net/wwan/wwan_core.c

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