3 files changed, 470 insertions, 0 deletions

diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index a906d3998131..c47237c2d638 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -369,4 +369,12 @@ config USB_NET_INT51X1
           (Powerline Communications) solution with an Intellon
           INT51x1/INT5200 chip, like the "devolo dLan duo".
 
+config USB_CDC_PHONET
+        tristate "CDC Phonet support"
+        depends on PHONET
+        help
+          Choose this option to support the Phonet interface to a Nokia
+          cellular modem, as found on most Nokia handsets with the
+          "PC suite" USB profile.
+
 endmenu
diff --git a/drivers/net/usb/Makefile b/drivers/net/usb/Makefile
index b870b0b1cbe0..e17afb78f372 100644
--- a/drivers/net/usb/Makefile
+++ b/drivers/net/usb/Makefile
@@ -21,4 +21,5 @@ obj-$(CONFIG_USB_NET_ZAURUS)	+= zaurus.o
 obj-$(CONFIG_USB_NET_MCS7830)   += mcs7830.o
 obj-$(CONFIG_USB_USBNET)        += usbnet.o
 obj-$(CONFIG_USB_NET_INT51X1)   += int51x1.o
+obj-$(CONFIG_USB_CDC_PHONET)    += cdc-phonet.o
 
diff --git a/drivers/net/usb/cdc-phonet.c b/drivers/net/usb/cdc-phonet.c
new file mode 100644
index 000000000000..792af72da8ac
--- /dev/null
+++ b/drivers/net/usb/cdc-phonet.c
@@ -0,0 +1,461 @@
+/*
+ * phonet.c -- USB CDC Phonet host driver
+ *
+ * Copyright (C) 2008-2009 Nokia Corporation. All rights reserved.
+ *
+ * Author: Rémi Denis-Courmont
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/usb/cdc.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_phonet.h>
+
+#define PN_MEDIA_USB    0x1B
+
+static const unsigned rxq_size = 17;
+
+struct usbpn_dev {
+        struct net_device       *dev;
+
+        struct usb_interface    *intf, *data_intf;
+        struct usb_device       *usb;
+        unsigned int            tx_pipe, rx_pipe;
+        u8 active_setting;
+        u8 disconnected;
+
+        unsigned                tx_queue;
+        spinlock_t              tx_lock;
+
+        spinlock_t              rx_lock;
+        struct sk_buff          *rx_skb;
+        struct urb              *urbs[0];
+};
+
+static void tx_complete(struct urb *req);
+static void rx_complete(struct urb *req);
+
+/*
+ * Network device callbacks
+ */
+static int usbpn_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+        struct usbpn_dev *pnd = netdev_priv(dev);
+        struct urb *req = NULL;
+        unsigned long flags;
+        int err;
+
+        if (skb->protocol != htons(ETH_P_PHONET))
+                goto drop;
+
+        req = usb_alloc_urb(0, GFP_ATOMIC);
+        if (!req)
+                goto drop;
+        usb_fill_bulk_urb(req, pnd->usb, pnd->tx_pipe, skb->data, skb->len,
+                                tx_complete, skb);
+        req->transfer_flags = URB_ZERO_PACKET;
+        err = usb_submit_urb(req, GFP_ATOMIC);
+        if (err) {
+                usb_free_urb(req);
+                goto drop;
+        }
+
+        spin_lock_irqsave(&pnd->tx_lock, flags);
+        pnd->tx_queue++;
+        if (pnd->tx_queue >= dev->tx_queue_len)
+                netif_stop_queue(dev);
+        spin_unlock_irqrestore(&pnd->tx_lock, flags);
+        return 0;
+
+drop:
+        dev_kfree_skb(skb);
+        dev->stats.tx_dropped++;
+        return 0;
+}
+
+static void tx_complete(struct urb *req)
+{
+        struct sk_buff *skb = req->context;
+        struct net_device *dev = skb->dev;
+        struct usbpn_dev *pnd = netdev_priv(dev);
+
+        switch (req->status) {
+        case 0:
+                dev->stats.tx_bytes += skb->len;
+                break;
+
+        case -ENOENT:
+        case -ECONNRESET:
+        case -ESHUTDOWN:
+                dev->stats.tx_aborted_errors++;
+        default:
+                dev->stats.tx_errors++;
+                dev_dbg(&dev->dev, "TX error (%d)\n", req->status);
+        }
+        dev->stats.tx_packets++;
+
+        spin_lock(&pnd->tx_lock);
+        pnd->tx_queue--;
+        netif_wake_queue(dev);
+        spin_unlock(&pnd->tx_lock);
+
+        dev_kfree_skb_any(skb);
+        usb_free_urb(req);
+}
+
+static int rx_submit(struct usbpn_dev *pnd, struct urb *req, gfp_t gfp_flags)
+{
+        struct net_device *dev = pnd->dev;
+        struct page *page;
+        int err;
+
+        page = __netdev_alloc_page(dev, gfp_flags);
+        if (!page)
+                return -ENOMEM;
+
+        usb_fill_bulk_urb(req, pnd->usb, pnd->rx_pipe, page_address(page),
+                                PAGE_SIZE, rx_complete, dev);
+        req->transfer_flags = 0;
+        err = usb_submit_urb(req, gfp_flags);
+        if (unlikely(err)) {
+                dev_dbg(&dev->dev, "RX submit error (%d)\n", err);
+                netdev_free_page(dev, page);
+        }
+        return err;
+}
+
+static void rx_complete(struct urb *req)
+{
+        struct net_device *dev = req->context;
+        struct usbpn_dev *pnd = netdev_priv(dev);
+        struct page *page = virt_to_page(req->transfer_buffer);
+        struct sk_buff *skb;
+        unsigned long flags;
+
+        switch (req->status) {
+        case 0:
+                spin_lock_irqsave(&pnd->rx_lock, flags);
+                skb = pnd->rx_skb;
+                if (!skb) {
+                        skb = pnd->rx_skb = netdev_alloc_skb(dev, 12);
+                        if (likely(skb)) {
+                                /* Can't use pskb_pull() on page in IRQ */
+                                memcpy(skb_put(skb, 1), page_address(page), 1);
+                                skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
+                                                page, 1, req->actual_length);
+                                page = NULL;
+                        }
+                } else {
+                        skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
+                                        page, 0, req->actual_length);
+                        page = NULL;
+                }
+                if (req->actual_length < PAGE_SIZE)
+                        pnd->rx_skb = NULL; /* Last fragment */
+                else
+                        skb = NULL;
+                spin_unlock_irqrestore(&pnd->rx_lock, flags);
+                if (skb) {
+                        skb->protocol = htons(ETH_P_PHONET);
+                        skb_reset_mac_header(skb);
+                        __skb_pull(skb, 1);
+                        skb->dev = dev;
+                        dev->stats.rx_packets++;
+                        dev->stats.rx_bytes += skb->len;
+
+                        netif_rx(skb);
+                }
+                goto resubmit;
+
+        case -ENOENT:
+        case -ECONNRESET:
+        case -ESHUTDOWN:
+                req = NULL;
+                break;
+
+        case -EOVERFLOW:
+                dev->stats.rx_over_errors++;
+                dev_dbg(&dev->dev, "RX overflow\n");
+                break;
+
+        case -EILSEQ:
+                dev->stats.rx_crc_errors++;
+                break;
+        }
+
+        dev->stats.rx_errors++;
+resubmit:
+        if (page)
+                netdev_free_page(dev, page);
+        if (req)
+                rx_submit(pnd, req, GFP_ATOMIC);
+}
+
+static int usbpn_close(struct net_device *dev);
+
+static int usbpn_open(struct net_device *dev)
+{
+        struct usbpn_dev *pnd = netdev_priv(dev);
+        int err;
+        unsigned i;
+        unsigned num = pnd->data_intf->cur_altsetting->desc.bInterfaceNumber;
+
+        err = usb_set_interface(pnd->usb, num, pnd->active_setting);
+        if (err)
+                return err;
+
+        for (i = 0; i < rxq_size; i++) {
+                struct urb *req = usb_alloc_urb(0, GFP_KERNEL);
+
+                if (!req || rx_submit(pnd, req, GFP_KERNEL)) {
+                        usbpn_close(dev);
+                        return -ENOMEM;
+                }
+                pnd->urbs[i] = req;
+        }
+
+        netif_wake_queue(dev);
+        return 0;
+}
+
+static int usbpn_close(struct net_device *dev)
+{
+        struct usbpn_dev *pnd = netdev_priv(dev);
+        unsigned i;
+        unsigned num = pnd->data_intf->cur_altsetting->desc.bInterfaceNumber;
+
+        netif_stop_queue(dev);
+
+        for (i = 0; i < rxq_size; i++) {
+                struct urb *req = pnd->urbs[i];
+
+                if (!req)
+                        continue;
+                usb_kill_urb(req);
+                usb_free_urb(req);
+                pnd->urbs[i] = NULL;
+        }
+
+        return usb_set_interface(pnd->usb, num, !pnd->active_setting);
+}
+
+static int usbpn_set_mtu(struct net_device *dev, int new_mtu)
+{
+        if ((new_mtu < PHONET_MIN_MTU) || (new_mtu > PHONET_MAX_MTU))
+                return -EINVAL;
+
+        dev->mtu = new_mtu;
+        return 0;
+}
+
+static const struct net_device_ops usbpn_ops = {
+        .ndo_open       = usbpn_open,
+        .ndo_stop       = usbpn_close,
+        .ndo_start_xmit = usbpn_xmit,
+        .ndo_change_mtu = usbpn_set_mtu,
+};
+
+static void usbpn_setup(struct net_device *dev)
+{
+        dev->features           = 0;
+        dev->netdev_ops         = &usbpn_ops,
+        dev->header_ops         = &phonet_header_ops;
+        dev->type               = ARPHRD_PHONET;
+        dev->flags              = IFF_POINTOPOINT | IFF_NOARP;
+        dev->mtu                = PHONET_MAX_MTU;
+        dev->hard_header_len    = 1;
+        dev->dev_addr[0]        = PN_MEDIA_USB;
+        dev->addr_len           = 1;
+        dev->tx_queue_len       = 3;
+
+        dev->destructor         = free_netdev;
+}
+
+/*
+ * USB driver callbacks
+ */
+static struct usb_device_id usbpn_ids[] = {
+        {
+                .match_flags = USB_DEVICE_ID_MATCH_VENDOR
+                        | USB_DEVICE_ID_MATCH_INT_CLASS
+                        | USB_DEVICE_ID_MATCH_INT_SUBCLASS,
+                .idVendor = 0x0421, /* Nokia */
+                .bInterfaceClass = USB_CLASS_COMM,
+                .bInterfaceSubClass = 0xFE,
+        },
+        { },
+};
+
+MODULE_DEVICE_TABLE(usb, usbpn_ids);
+
+static struct usb_driver usbpn_driver;
+
+int usbpn_probe(struct usb_interface *intf, const struct usb_device_id *id)
+{
+        static const char ifname[] = "usbpn%d";
+        const struct usb_cdc_union_desc *union_header = NULL;
+        const struct usb_cdc_header_desc *phonet_header = NULL;
+        const struct usb_host_interface *data_desc;
+        struct usb_interface *data_intf;
+        struct usb_device *usbdev = interface_to_usbdev(intf);
+        struct net_device *dev;
+        struct usbpn_dev *pnd;
+        u8 *data;
+        int len, err;
+
+        data = intf->altsetting->extra;
+        len = intf->altsetting->extralen;
+        while (len >= 3) {
+                u8 dlen = data[0];
+                if (dlen < 3)
+                        return -EINVAL;
+
+                /* bDescriptorType */
+                if (data[1] == USB_DT_CS_INTERFACE) {
+                        /* bDescriptorSubType */
+                        switch (data[2]) {
+                        case USB_CDC_UNION_TYPE:
+                                if (union_header || dlen < 5)
+                                        break;
+                                union_header =
+                                        (struct usb_cdc_union_desc *)data;
+                                break;
+                        case 0xAB:
+                                if (phonet_header || dlen < 5)
+                                        break;
+                                phonet_header =
+                                        (struct usb_cdc_header_desc *)data;
+                                break;
+                        }
+                }
+                data += dlen;
+                len -= dlen;
+        }
+
+        if (!union_header || !phonet_header)
+                return -EINVAL;
+
+        data_intf = usb_ifnum_to_if(usbdev, union_header->bSlaveInterface0);
+        if (data_intf == NULL)
+                return -ENODEV;
+        /* Data interface has one inactive and one active setting */
+        if (data_intf->num_altsetting != 2)
+                return -EINVAL;
+        if (data_intf->altsetting[0].desc.bNumEndpoints == 0
+         && data_intf->altsetting[1].desc.bNumEndpoints == 2)
+                data_desc = data_intf->altsetting + 1;
+        else
+        if (data_intf->altsetting[0].desc.bNumEndpoints == 2
+         && data_intf->altsetting[1].desc.bNumEndpoints == 0)
+                data_desc = data_intf->altsetting;
+        else
+                return -EINVAL;
+
+        dev = alloc_netdev(sizeof(*pnd) + sizeof(pnd->urbs[0]) * rxq_size,
+                                ifname, usbpn_setup);
+        if (!dev)
+                return -ENOMEM;
+
+        pnd = netdev_priv(dev);
+        SET_NETDEV_DEV(dev, &intf->dev);
+        netif_stop_queue(dev);
+
+        pnd->dev = dev;
+        pnd->usb = usb_get_dev(usbdev);
+        pnd->intf = intf;
+        pnd->data_intf = data_intf;
+        spin_lock_init(&pnd->tx_lock);
+        spin_lock_init(&pnd->rx_lock);
+        /* Endpoints */
+        if (usb_pipein(data_desc->endpoint[0].desc.bEndpointAddress)) {
+                pnd->rx_pipe = usb_rcvbulkpipe(usbdev,
+                        data_desc->endpoint[0].desc.bEndpointAddress);
+                pnd->tx_pipe = usb_sndbulkpipe(usbdev,
+                        data_desc->endpoint[1].desc.bEndpointAddress);
+        } else {
+                pnd->rx_pipe = usb_rcvbulkpipe(usbdev,
+                        data_desc->endpoint[1].desc.bEndpointAddress);
+                pnd->tx_pipe = usb_sndbulkpipe(usbdev,
+                        data_desc->endpoint[0].desc.bEndpointAddress);
+        }
+        pnd->active_setting = data_desc - data_intf->altsetting;
+
+        err = usb_driver_claim_interface(&usbpn_driver, data_intf, pnd);
+        if (err)
+                goto out;
+
+        /* Force inactive mode until the network device is brought UP */
+        usb_set_interface(usbdev, union_header->bSlaveInterface0,
+                                !pnd->active_setting);
+        usb_set_intfdata(intf, pnd);
+
+        err = register_netdev(dev);
+        if (err) {
+                usb_driver_release_interface(&usbpn_driver, data_intf);
+                goto out;
+        }
+
+        dev_dbg(&dev->dev, "USB CDC Phonet device found\n");
+        return 0;
+
+out:
+        usb_set_intfdata(intf, NULL);
+        free_netdev(dev);
+        return err;
+}
+
+static void usbpn_disconnect(struct usb_interface *intf)
+{
+        struct usbpn_dev *pnd = usb_get_intfdata(intf);
+        struct usb_device *usb = pnd->usb;
+
+        if (pnd->disconnected)
+                return;
+
+        pnd->disconnected = 1;
+        usb_driver_release_interface(&usbpn_driver,
+                        (pnd->intf == intf) ? pnd->data_intf : pnd->intf);
+        unregister_netdev(pnd->dev);
+        usb_put_dev(usb);
+}
+
+static struct usb_driver usbpn_driver = {
+        .name =         "cdc_phonet",
+        .probe =        usbpn_probe,
+        .disconnect =   usbpn_disconnect,
+        .id_table =     usbpn_ids,
+};
+
+static int __init usbpn_init(void)
+{
+        return usb_register(&usbpn_driver);
+}
+
+static void __exit usbpn_exit(void)
+{
+        usb_deregister(&usbpn_driver);
+}
+
+module_init(usbpn_init);
+module_exit(usbpn_exit);
+
+MODULE_AUTHOR("Remi Denis-Courmont");
+MODULE_DESCRIPTION("USB CDC Phonet host interface");
+MODULE_LICENSE("GPL");