cdc-phonet.c

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