[go: nahoru, domu]
1/*
2 * phonet.c -- USB CDC Phonet host driver
3 *
4 * Copyright (C) 2008-2009 Nokia Corporation. All rights reserved.
5 *
6 * Author: Rémi Denis-Courmont
7 *
8 * This program is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU General Public License
10 * version 2 as published by the Free Software Foundation.
11 *
12 * This program is distributed in the hope that it will be useful, but
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15 * General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
20 * 02110-1301 USA
21 */
22
23#include <linux/kernel.h>
24#include <linux/mm.h>
25#include <linux/module.h>
26#include <linux/gfp.h>
27#include <linux/usb.h>
28#include <linux/usb/cdc.h>
29#include <linux/netdevice.h>
30#include <linux/if_arp.h>
31#include <linux/if_phonet.h>
32#include <linux/phonet.h>
33
34#define PN_MEDIA_USB	0x1B
35
36static const unsigned rxq_size = 17;
37
38struct usbpn_dev {
39	struct net_device	*dev;
40
41	struct usb_interface	*intf, *data_intf;
42	struct usb_device	*usb;
43	unsigned int		tx_pipe, rx_pipe;
44	u8 active_setting;
45	u8 disconnected;
46
47	unsigned		tx_queue;
48	spinlock_t		tx_lock;
49
50	spinlock_t		rx_lock;
51	struct sk_buff		*rx_skb;
52	struct urb		*urbs[0];
53};
54
55static void tx_complete(struct urb *req);
56static void rx_complete(struct urb *req);
57
58/*
59 * Network device callbacks
60 */
61static netdev_tx_t usbpn_xmit(struct sk_buff *skb, struct net_device *dev)
62{
63	struct usbpn_dev *pnd = netdev_priv(dev);
64	struct urb *req = NULL;
65	unsigned long flags;
66	int err;
67
68	if (skb->protocol != htons(ETH_P_PHONET))
69		goto drop;
70
71	req = usb_alloc_urb(0, GFP_ATOMIC);
72	if (!req)
73		goto drop;
74	usb_fill_bulk_urb(req, pnd->usb, pnd->tx_pipe, skb->data, skb->len,
75				tx_complete, skb);
76	req->transfer_flags = URB_ZERO_PACKET;
77	err = usb_submit_urb(req, GFP_ATOMIC);
78	if (err) {
79		usb_free_urb(req);
80		goto drop;
81	}
82
83	spin_lock_irqsave(&pnd->tx_lock, flags);
84	pnd->tx_queue++;
85	if (pnd->tx_queue >= dev->tx_queue_len)
86		netif_stop_queue(dev);
87	spin_unlock_irqrestore(&pnd->tx_lock, flags);
88	return NETDEV_TX_OK;
89
90drop:
91	dev_kfree_skb(skb);
92	dev->stats.tx_dropped++;
93	return NETDEV_TX_OK;
94}
95
96static void tx_complete(struct urb *req)
97{
98	struct sk_buff *skb = req->context;
99	struct net_device *dev = skb->dev;
100	struct usbpn_dev *pnd = netdev_priv(dev);
101	int status = req->status;
102
103	switch (status) {
104	case 0:
105		dev->stats.tx_bytes += skb->len;
106		break;
107
108	case -ENOENT:
109	case -ECONNRESET:
110	case -ESHUTDOWN:
111		dev->stats.tx_aborted_errors++;
112	default:
113		dev->stats.tx_errors++;
114		dev_dbg(&dev->dev, "TX error (%d)\n", status);
115	}
116	dev->stats.tx_packets++;
117
118	spin_lock(&pnd->tx_lock);
119	pnd->tx_queue--;
120	netif_wake_queue(dev);
121	spin_unlock(&pnd->tx_lock);
122
123	dev_kfree_skb_any(skb);
124	usb_free_urb(req);
125}
126
127static int rx_submit(struct usbpn_dev *pnd, struct urb *req, gfp_t gfp_flags)
128{
129	struct net_device *dev = pnd->dev;
130	struct page *page;
131	int err;
132
133	page = __skb_alloc_page(gfp_flags | __GFP_NOMEMALLOC, NULL);
134	if (!page)
135		return -ENOMEM;
136
137	usb_fill_bulk_urb(req, pnd->usb, pnd->rx_pipe, page_address(page),
138				PAGE_SIZE, rx_complete, dev);
139	req->transfer_flags = 0;
140	err = usb_submit_urb(req, gfp_flags);
141	if (unlikely(err)) {
142		dev_dbg(&dev->dev, "RX submit error (%d)\n", err);
143		put_page(page);
144	}
145	return err;
146}
147
148static void rx_complete(struct urb *req)
149{
150	struct net_device *dev = req->context;
151	struct usbpn_dev *pnd = netdev_priv(dev);
152	struct page *page = virt_to_page(req->transfer_buffer);
153	struct sk_buff *skb;
154	unsigned long flags;
155	int status = req->status;
156
157	switch (status) {
158	case 0:
159		spin_lock_irqsave(&pnd->rx_lock, flags);
160		skb = pnd->rx_skb;
161		if (!skb) {
162			skb = pnd->rx_skb = netdev_alloc_skb(dev, 12);
163			if (likely(skb)) {
164				/* Can't use pskb_pull() on page in IRQ */
165				memcpy(skb_put(skb, 1), page_address(page), 1);
166				skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
167						page, 1, req->actual_length,
168						PAGE_SIZE);
169				page = NULL;
170			}
171		} else {
172			skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
173					page, 0, req->actual_length,
174					PAGE_SIZE);
175			page = NULL;
176		}
177		if (req->actual_length < PAGE_SIZE)
178			pnd->rx_skb = NULL; /* Last fragment */
179		else
180			skb = NULL;
181		spin_unlock_irqrestore(&pnd->rx_lock, flags);
182		if (skb) {
183			skb->protocol = htons(ETH_P_PHONET);
184			skb_reset_mac_header(skb);
185			__skb_pull(skb, 1);
186			skb->dev = dev;
187			dev->stats.rx_packets++;
188			dev->stats.rx_bytes += skb->len;
189
190			netif_rx(skb);
191		}
192		goto resubmit;
193
194	case -ENOENT:
195	case -ECONNRESET:
196	case -ESHUTDOWN:
197		req = NULL;
198		break;
199
200	case -EOVERFLOW:
201		dev->stats.rx_over_errors++;
202		dev_dbg(&dev->dev, "RX overflow\n");
203		break;
204
205	case -EILSEQ:
206		dev->stats.rx_crc_errors++;
207		break;
208	}
209
210	dev->stats.rx_errors++;
211resubmit:
212	if (page)
213		put_page(page);
214	if (req)
215		rx_submit(pnd, req, GFP_ATOMIC | __GFP_COLD);
216}
217
218static int usbpn_close(struct net_device *dev);
219
220static int usbpn_open(struct net_device *dev)
221{
222	struct usbpn_dev *pnd = netdev_priv(dev);
223	int err;
224	unsigned i;
225	unsigned num = pnd->data_intf->cur_altsetting->desc.bInterfaceNumber;
226
227	err = usb_set_interface(pnd->usb, num, pnd->active_setting);
228	if (err)
229		return err;
230
231	for (i = 0; i < rxq_size; i++) {
232		struct urb *req = usb_alloc_urb(0, GFP_KERNEL);
233
234		if (!req || rx_submit(pnd, req, GFP_KERNEL | __GFP_COLD)) {
235			usb_free_urb(req);
236			usbpn_close(dev);
237			return -ENOMEM;
238		}
239		pnd->urbs[i] = req;
240	}
241
242	netif_wake_queue(dev);
243	return 0;
244}
245
246static int usbpn_close(struct net_device *dev)
247{
248	struct usbpn_dev *pnd = netdev_priv(dev);
249	unsigned i;
250	unsigned num = pnd->data_intf->cur_altsetting->desc.bInterfaceNumber;
251
252	netif_stop_queue(dev);
253
254	for (i = 0; i < rxq_size; i++) {
255		struct urb *req = pnd->urbs[i];
256
257		if (!req)
258			continue;
259		usb_kill_urb(req);
260		usb_free_urb(req);
261		pnd->urbs[i] = NULL;
262	}
263
264	return usb_set_interface(pnd->usb, num, !pnd->active_setting);
265}
266
267static int usbpn_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
268{
269	struct if_phonet_req *req = (struct if_phonet_req *)ifr;
270
271	switch (cmd) {
272	case SIOCPNGAUTOCONF:
273		req->ifr_phonet_autoconf.device = PN_DEV_PC;
274		return 0;
275	}
276	return -ENOIOCTLCMD;
277}
278
279static int usbpn_set_mtu(struct net_device *dev, int new_mtu)
280{
281	if ((new_mtu < PHONET_MIN_MTU) || (new_mtu > PHONET_MAX_MTU))
282		return -EINVAL;
283
284	dev->mtu = new_mtu;
285	return 0;
286}
287
288static const struct net_device_ops usbpn_ops = {
289	.ndo_open	= usbpn_open,
290	.ndo_stop	= usbpn_close,
291	.ndo_start_xmit = usbpn_xmit,
292	.ndo_do_ioctl	= usbpn_ioctl,
293	.ndo_change_mtu = usbpn_set_mtu,
294};
295
296static void usbpn_setup(struct net_device *dev)
297{
298	dev->features		= 0;
299	dev->netdev_ops		= &usbpn_ops,
300	dev->header_ops		= &phonet_header_ops;
301	dev->type		= ARPHRD_PHONET;
302	dev->flags		= IFF_POINTOPOINT | IFF_NOARP;
303	dev->mtu		= PHONET_MAX_MTU;
304	dev->hard_header_len	= 1;
305	dev->dev_addr[0]	= PN_MEDIA_USB;
306	dev->addr_len		= 1;
307	dev->tx_queue_len	= 3;
308
309	dev->destructor		= free_netdev;
310}
311
312/*
313 * USB driver callbacks
314 */
315static struct usb_device_id usbpn_ids[] = {
316	{
317		.match_flags = USB_DEVICE_ID_MATCH_VENDOR
318			| USB_DEVICE_ID_MATCH_INT_CLASS
319			| USB_DEVICE_ID_MATCH_INT_SUBCLASS,
320		.idVendor = 0x0421, /* Nokia */
321		.bInterfaceClass = USB_CLASS_COMM,
322		.bInterfaceSubClass = 0xFE,
323	},
324	{ },
325};
326
327MODULE_DEVICE_TABLE(usb, usbpn_ids);
328
329static struct usb_driver usbpn_driver;
330
331static int usbpn_probe(struct usb_interface *intf, const struct usb_device_id *id)
332{
333	static const char ifname[] = "usbpn%d";
334	const struct usb_cdc_union_desc *union_header = NULL;
335	const struct usb_host_interface *data_desc;
336	struct usb_interface *data_intf;
337	struct usb_device *usbdev = interface_to_usbdev(intf);
338	struct net_device *dev;
339	struct usbpn_dev *pnd;
340	u8 *data;
341	int phonet = 0;
342	int len, err;
343
344	data = intf->altsetting->extra;
345	len = intf->altsetting->extralen;
346	while (len >= 3) {
347		u8 dlen = data[0];
348		if (dlen < 3)
349			return -EINVAL;
350
351		/* bDescriptorType */
352		if (data[1] == USB_DT_CS_INTERFACE) {
353			/* bDescriptorSubType */
354			switch (data[2]) {
355			case USB_CDC_UNION_TYPE:
356				if (union_header || dlen < 5)
357					break;
358				union_header =
359					(struct usb_cdc_union_desc *)data;
360				break;
361			case 0xAB:
362				phonet = 1;
363				break;
364			}
365		}
366		data += dlen;
367		len -= dlen;
368	}
369
370	if (!union_header || !phonet)
371		return -EINVAL;
372
373	data_intf = usb_ifnum_to_if(usbdev, union_header->bSlaveInterface0);
374	if (data_intf == NULL)
375		return -ENODEV;
376	/* Data interface has one inactive and one active setting */
377	if (data_intf->num_altsetting != 2)
378		return -EINVAL;
379	if (data_intf->altsetting[0].desc.bNumEndpoints == 0 &&
380	    data_intf->altsetting[1].desc.bNumEndpoints == 2)
381		data_desc = data_intf->altsetting + 1;
382	else
383	if (data_intf->altsetting[0].desc.bNumEndpoints == 2 &&
384	    data_intf->altsetting[1].desc.bNumEndpoints == 0)
385		data_desc = data_intf->altsetting;
386	else
387		return -EINVAL;
388
389	dev = alloc_netdev(sizeof(*pnd) + sizeof(pnd->urbs[0]) * rxq_size,
390			   ifname, NET_NAME_UNKNOWN, usbpn_setup);
391	if (!dev)
392		return -ENOMEM;
393
394	pnd = netdev_priv(dev);
395	SET_NETDEV_DEV(dev, &intf->dev);
396
397	pnd->dev = dev;
398	pnd->usb = usbdev;
399	pnd->intf = intf;
400	pnd->data_intf = data_intf;
401	spin_lock_init(&pnd->tx_lock);
402	spin_lock_init(&pnd->rx_lock);
403	/* Endpoints */
404	if (usb_pipein(data_desc->endpoint[0].desc.bEndpointAddress)) {
405		pnd->rx_pipe = usb_rcvbulkpipe(usbdev,
406			data_desc->endpoint[0].desc.bEndpointAddress);
407		pnd->tx_pipe = usb_sndbulkpipe(usbdev,
408			data_desc->endpoint[1].desc.bEndpointAddress);
409	} else {
410		pnd->rx_pipe = usb_rcvbulkpipe(usbdev,
411			data_desc->endpoint[1].desc.bEndpointAddress);
412		pnd->tx_pipe = usb_sndbulkpipe(usbdev,
413			data_desc->endpoint[0].desc.bEndpointAddress);
414	}
415	pnd->active_setting = data_desc - data_intf->altsetting;
416
417	err = usb_driver_claim_interface(&usbpn_driver, data_intf, pnd);
418	if (err)
419		goto out;
420
421	/* Force inactive mode until the network device is brought UP */
422	usb_set_interface(usbdev, union_header->bSlaveInterface0,
423				!pnd->active_setting);
424	usb_set_intfdata(intf, pnd);
425
426	err = register_netdev(dev);
427	if (err) {
428		usb_driver_release_interface(&usbpn_driver, data_intf);
429		goto out;
430	}
431
432	dev_dbg(&dev->dev, "USB CDC Phonet device found\n");
433	return 0;
434
435out:
436	usb_set_intfdata(intf, NULL);
437	free_netdev(dev);
438	return err;
439}
440
441static void usbpn_disconnect(struct usb_interface *intf)
442{
443	struct usbpn_dev *pnd = usb_get_intfdata(intf);
444
445	if (pnd->disconnected)
446		return;
447
448	pnd->disconnected = 1;
449	usb_driver_release_interface(&usbpn_driver,
450			(pnd->intf == intf) ? pnd->data_intf : pnd->intf);
451	unregister_netdev(pnd->dev);
452}
453
454static struct usb_driver usbpn_driver = {
455	.name =		"cdc_phonet",
456	.probe =	usbpn_probe,
457	.disconnect =	usbpn_disconnect,
458	.id_table =	usbpn_ids,
459	.disable_hub_initiated_lpm = 1,
460};
461
462module_usb_driver(usbpn_driver);
463
464MODULE_AUTHOR("Remi Denis-Courmont");
465MODULE_DESCRIPTION("USB CDC Phonet host interface");
466MODULE_LICENSE("GPL");
467