[PATCH V2] USB: usbtmc: Add support for missing USBTMC-USB488 spec

[dave penkler <dpenkler@gmail.com>]

Implement support for the USB488 defined READ_STATUS_BYTE and SRQ
notifications with ioctl, fasync and poll/select in order to be able
to synchronize with variable duration instrument operations.

Add ioctls for other USB488 requests: REN_CONTROL, GOTO_LOCAL and
LOCAL_LOCKOUT.

Add convenience ioctl to return all device capabilities.

Fix PATCH V1 bug: not waking sleepers on disconnect.

Correct sparse warnings.

Signed-off-by: Dave Penkler <dpenkler@gmail.com>
---
 drivers/usb/class/usbtmc.c   | 359 +++++++++++++++++++++++++++++++++++++++++++
 include/uapi/linux/usb/tmc.h |  42 +++--
 2 files changed, 391 insertions(+), 10 deletions(-)

diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index 7a11a82..48e6efc 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -27,6 +27,7 @@
 #include <linux/uaccess.h>
 #include <linux/kref.h>
 #include <linux/slab.h>
+#include <linux/poll.h>
 #include <linux/mutex.h>
 #include <linux/usb.h>
 #include <linux/usb/tmc.h>
@@ -87,6 +88,24 @@ struct usbtmc_device_data {
 	u8 bTag_last_write;	/* needed for abort */
 	u8 bTag_last_read;	/* needed for abort */
 
+
+	/* data for interrupt in endpoint handling */
+	u8             bNotify1;
+	u8             bNotify2;
+	u16            ifnum;
+	u8             iin_bTag;
+	u8            *iin_buffer;
+	atomic_t       iin_data_valid;
+	unsigned int   iin_ep;
+	int            iin_ep_present;
+	int            iin_interval;
+	struct urb    *iin_urb;
+	u16            iin_wMaxPacketSize;
+	atomic_t       srq_asserted;
+
+	/* coalesced usb488_caps from usbtmc_dev_capabilities */
+	u8 usb488_caps;
+
 	u8 rigol_quirk;
 
 	/* attributes from the USB TMC spec for this device */
@@ -99,6 +118,8 @@ struct usbtmc_device_data {
 	struct usbtmc_dev_capabilities	capabilities;
 	struct kref kref;
 	struct mutex io_mutex;	/* only one i/o function running at a time */
+	wait_queue_head_t waitq;
+	struct fasync_struct *fasync;
 };
 #define to_usbtmc_data(d) container_of(d, struct usbtmc_device_data, kref)
 
@@ -373,6 +394,157 @@ exit:
 	return rv;
 }
 
+static int usbtmc488_ioctl_read_stb(struct usbtmc_device_data *data,
+				unsigned long arg)
+{
+	u8 *buffer;
+	struct device *dev;
+	int rv;
+	u8 tag, stb;
+
+	dev = &data->intf->dev;
+
+	dev_dbg(dev, "Enter ioctl_read_stb iin_ep_present: %d\n",
+		data->iin_ep_present);
+
+	buffer = kmalloc(8, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+
+	atomic_set(&data->iin_data_valid, 0);
+
+	/* must issue read_stb before using poll or select */
+	atomic_set(&data->srq_asserted, 0);
+
+	rv = usb_control_msg(data->usb_dev,
+			usb_rcvctrlpipe(data->usb_dev, 0),
+			USBTMC488_REQUEST_READ_STATUS_BYTE,
+			USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+			data->iin_bTag,
+			data->ifnum,
+			buffer, 0x03, USBTMC_TIMEOUT);
+
+	if (rv < 0) {
+		dev_err(dev, "stb usb_control_msg returned %d\n", rv);
+		goto exit;
+	}
+
+	if (buffer[0] != USBTMC_STATUS_SUCCESS) {
+		dev_err(dev, "control status returned %x\n",
+			buffer[0]);
+		rv = -EIO;
+		goto exit;
+	}
+
+	if (data->iin_ep_present) {
+
+		rv = wait_event_interruptible_timeout(
+			data->waitq,
+			(atomic_read(&data->iin_data_valid) != 0),
+			USBTMC_TIMEOUT
+			);
+
+		if (rv < 0) {
+			dev_dbg(dev, "wait interrupted %d\n", rv);
+			goto exit;
+		}
+
+		if (rv == 0) {
+			dev_dbg(dev, "wait timed out\n");
+			rv = -ETIME;
+			goto exit;
+		}
+
+		tag = data->bNotify1 & 0x7f;
+
+		if (tag != data->iin_bTag) {
+			dev_err(dev, "expected bTag %x got %x\n",
+				data->iin_bTag, tag);
+		}
+
+		stb = data->bNotify2;
+	} else {
+		stb = buffer[2];
+	}
+
+	rv = copy_to_user((void __user *)arg, &stb, sizeof(stb));
+	if (rv)
+		rv = -EFAULT;
+
+ exit:
+	/* bump interrupt bTag */
+	data->iin_bTag += 1;
+	if (data->iin_bTag > 127)
+		data->iin_bTag = 2;
+
+	kfree(buffer);
+	return rv;
+}
+
+static int usbtmc488_ioctl_simple(struct usbtmc_device_data *data,
+				unsigned long arg,
+				unsigned int cmd)
+{
+	u8 *buffer;
+	struct device *dev;
+	int rv;
+	unsigned int val;
+	u16 wValue;
+
+	dev = &data->intf->dev;
+
+	if (0 == (data->usb488_caps & USBTMC488_CAPABILITY_SIMPLE))
+		return -EINVAL;
+
+	buffer = kmalloc(8, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+
+	if (cmd == USBTMC488_REQUEST_REN_CONTROL) {
+		rv = copy_from_user(&val, (void __user *)arg, sizeof(val));
+		if (rv) {
+			rv = -EFAULT;
+			goto exit;
+		}
+		wValue = (val) ? 1 : 0;
+	} else {
+		wValue = 0;
+	}
+
+	rv = usb_control_msg(data->usb_dev,
+			usb_rcvctrlpipe(data->usb_dev, 0),
+			cmd,
+			USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+			wValue,
+			data->ifnum,
+			buffer, 0x01, USBTMC_TIMEOUT);
+
+	if (rv < 0) {
+		dev_err(dev, "simple usb_control_msg failed %d\n", rv);
+		goto exit;
+	} else if (rv != 1) {
+		dev_warn(dev, "simple usb_control_msg returned %d\n", rv);
+		rv = -EIO;
+		goto exit;
+	}
+
+	if (buffer[0] != USBTMC_STATUS_SUCCESS) {
+		dev_err(dev, "simple control status returned %x\n",
+			buffer[0]);
+		rv = -EIO;
+		goto exit;
+	} else {
+		rv = 0;
+	}
+
+ exit:
+	kfree(buffer);
+	return rv;
+}
+
+
 /*
  * Sends a REQUEST_DEV_DEP_MSG_IN message on the Bulk-IN endpoint.
  * @transfer_size: number of bytes to request from the device.
@@ -895,6 +1067,7 @@ static int get_capabilities(struct usbtmc_device_data *data)
 	data->capabilities.device_capabilities = buffer[5];
 	data->capabilities.usb488_interface_capabilities = buffer[14];
 	data->capabilities.usb488_device_capabilities = buffer[15];
+	data->usb488_caps = (buffer[14] & 0x07) | ((buffer[15] & 0x0f) << 4);
 	rv = 0;
 
 err_out:
@@ -1069,6 +1242,33 @@ static long usbtmc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case USBTMC_IOCTL_ABORT_BULK_IN:
 		retval = usbtmc_ioctl_abort_bulk_in(data);
 		break;
+
+	case USBTMC488_IOCTL_GET_CAPABILITIES:
+		retval = copy_to_user((void __user *)arg,
+				&data->usb488_caps,
+				sizeof(data->usb488_caps));
+		if (retval)
+			retval = -EFAULT;
+		break;
+
+	case USBTMC488_IOCTL_READ_STB:
+		retval = usbtmc488_ioctl_read_stb(data, arg);
+		break;
+
+	case USBTMC488_IOCTL_REN_CONTROL:
+		retval = usbtmc488_ioctl_simple(data, arg,
+						USBTMC488_REQUEST_REN_CONTROL);
+		break;
+
+	case USBTMC488_IOCTL_GOTO_LOCAL:
+		retval = usbtmc488_ioctl_simple(data, arg,
+						USBTMC488_REQUEST_GOTO_LOCAL);
+		break;
+
+	case USBTMC488_IOCTL_LOCAL_LOCKOUT:
+		retval = usbtmc488_ioctl_simple(data, arg,
+						USBTMC488_REQUEST_LOCAL_LOCKOUT);
+		break;
 	}
 
 skip_io_on_zombie:
@@ -1076,6 +1276,34 @@ skip_io_on_zombie:
 	return retval;
 }
 
+static int usbtmc_fasync(int fd, struct file *file, int on)
+{
+	struct usbtmc_device_data *data = file->private_data;
+
+	return fasync_helper(fd, file, on, &data->fasync);
+}
+
+static unsigned int usbtmc_poll(struct file *file, poll_table *wait)
+{
+	struct usbtmc_device_data *data = file->private_data;
+	unsigned int mask = 0;
+
+	mutex_lock(&data->io_mutex);
+
+	if (data->zombie) {
+		mask = POLLHUP | POLLERR;
+		goto no_poll;
+	}
+
+	poll_wait(file, &data->waitq, wait);
+
+	mask = (atomic_read(&data->srq_asserted)) ? POLLIN | POLLRDNORM : 0;
+
+no_poll:
+	mutex_unlock(&data->io_mutex);
+	return mask;
+}
+
 static const struct file_operations fops = {
 	.owner		= THIS_MODULE,
 	.read		= usbtmc_read,
@@ -1083,6 +1311,8 @@ static const struct file_operations fops = {
 	.open		= usbtmc_open,
 	.release	= usbtmc_release,
 	.unlocked_ioctl	= usbtmc_ioctl,
+	.fasync         = usbtmc_fasync,
+	.poll           = usbtmc_poll,
 	.llseek		= default_llseek,
 };
 
@@ -1092,6 +1322,79 @@ static struct usb_class_driver usbtmc_class = {
 	.minor_base =	USBTMC_MINOR_BASE,
 };
 
+static void usbtmc_interrupt(struct urb *urb)
+{
+	struct usbtmc_device_data *data = urb->context;
+	int status = urb->status;
+	int rv;
+
+	dev_dbg(&data->intf->dev, "int status: %d len %d\n",
+		status, urb->actual_length);
+
+	switch (status) {
+	case 0: /* SUCCESS */
+
+		if (data->iin_buffer[0] & 0x80) {
+			/* check for valid STB notification */
+			if ((data->iin_buffer[0] & 0x7f) > 1) {
+				data->bNotify1 = data->iin_buffer[0];
+				data->bNotify2 = data->iin_buffer[1];
+				atomic_set(&data->iin_data_valid, 1);
+				wake_up_interruptible(&data->waitq);
+				goto exit;
+			/* check for SRQ notification */
+			}
+			if ((data->iin_buffer[0] & 0x7f) == 1) {
+				if (data->fasync)
+					kill_fasync(&data->fasync,
+						SIGIO, POLL_IN);
+
+				atomic_set(&data->srq_asserted, 1);
+				wake_up_interruptible(&data->waitq);
+				goto exit;
+			}
+		}
+		dev_warn(&data->intf->dev, "invalid notification: %x\n",
+			data->iin_buffer[0]);
+		break;
+	case -EOVERFLOW:
+		dev_err(&data->intf->dev,
+			"%s - overflow with length %d, actual length is %d\n",
+			__func__, data->iin_wMaxPacketSize, urb->actual_length);
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -EILSEQ:
+	case -ETIME:
+		/* urb terminated, clean up */
+		dev_dbg(&data->intf->dev,
+			"%s - urb terminated, status: %d\n",
+			__func__, status);
+		return;
+	default:
+		dev_err(&data->intf->dev,
+			"%s - unknown status received: %d\n",
+			__func__, status);
+	}
+exit:
+	rv = usb_submit_urb(urb, GFP_ATOMIC);
+	if (rv) {
+		dev_err(&data->intf->dev, "%s - usb_submit_urb failed: %d\n",
+			__func__, rv);
+	}
+}
+
+static void usbtmc_free_int(struct usbtmc_device_data *data)
+{
+	if (data->iin_ep_present) {
+		if (data->iin_urb) {
+			usb_kill_urb(data->iin_urb);
+			kfree(data->iin_buffer);
+			usb_free_urb(data->iin_urb);
+			kref_put(&data->kref, usbtmc_delete);
+		}
+	}
+}
 
 static int usbtmc_probe(struct usb_interface *intf,
 			const struct usb_device_id *id)
@@ -1114,6 +1417,9 @@ static int usbtmc_probe(struct usb_interface *intf,
 	usb_set_intfdata(intf, data);
 	kref_init(&data->kref);
 	mutex_init(&data->io_mutex);
+	init_waitqueue_head(&data->waitq);
+	atomic_set(&data->iin_data_valid, 0);
+	atomic_set(&data->srq_asserted, 0);
 	data->zombie = 0;
 
 	/* Determine if it is a Rigol or not */
@@ -1134,9 +1440,12 @@ static int usbtmc_probe(struct usb_interface *intf,
 	data->bTag	= 1;
 	data->TermCharEnabled = 0;
 	data->TermChar = '\n';
+	/*  2 <= bTag <= 127   USBTMC-USB488 subclass specification 4.3.1 */
+	data->iin_bTag = 2;
 
 	/* USBTMC devices have only one setting, so use that */
 	iface_desc = data->intf->cur_altsetting;
+	data->ifnum = iface_desc->desc.bInterfaceNumber;
 
 	/* Find bulk in endpoint */
 	for (n = 0; n < iface_desc->desc.bNumEndpoints; n++) {
@@ -1161,6 +1470,20 @@ static int usbtmc_probe(struct usb_interface *intf,
 			break;
 		}
 	}
+	/* Find int endpoint */
+	for (n = 0; n < iface_desc->desc.bNumEndpoints; n++) {
+		endpoint = &iface_desc->endpoint[n].desc;
+
+		if (usb_endpoint_is_int_in(endpoint)) {
+			data->iin_ep_present = 1;
+			data->iin_ep = endpoint->bEndpointAddress;
+			data->iin_wMaxPacketSize = usb_endpoint_maxp(endpoint);
+			data->iin_interval = endpoint->bInterval;
+			dev_dbg(&intf->dev, "Found Int in endpoint at %u\n",
+				data->iin_ep);
+			break;
+		}
+	}
 
 	retcode = get_capabilities(data);
 	if (retcode)
@@ -1169,6 +1492,40 @@ static int usbtmc_probe(struct usb_interface *intf,
 		retcode = sysfs_create_group(&intf->dev.kobj,
 					     &capability_attr_grp);
 
+
+	if (data->iin_ep_present) {
+		/* allocate int urb */
+		data->iin_urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!data->iin_urb) {
+			dev_err(&intf->dev, "Failed to allocate int urb\n");
+			goto error_register;
+		}
+
+		/* will reference data in int urb */
+		kref_get(&data->kref);
+
+		/* allocate buffer for interrupt in */
+		data->iin_buffer = kmalloc(data->iin_wMaxPacketSize,
+					GFP_KERNEL);
+		if (!data->iin_buffer) {
+			dev_err(&intf->dev, "Failed to allocate int buf\n");
+			goto error_register;
+		}
+
+		/* fill interrupt urb */
+		usb_fill_int_urb(data->iin_urb, data->usb_dev,
+				usb_rcvintpipe(data->usb_dev, data->iin_ep),
+				data->iin_buffer, data->iin_wMaxPacketSize,
+				usbtmc_interrupt,
+				data, data->iin_interval);
+
+		if (usb_submit_urb(data->iin_urb, GFP_KERNEL)) {
+			retcode = -EIO;
+			dev_err(&intf->dev, "Failed to submit iin_urb\n");
+			goto error_register;
+		}
+	}
+
 	retcode = sysfs_create_group(&intf->dev.kobj, &data_attr_grp);
 
 	retcode = usb_register_dev(intf, &usbtmc_class);
@@ -1185,6 +1542,7 @@ static int usbtmc_probe(struct usb_interface *intf,
 error_register:
 	sysfs_remove_group(&intf->dev.kobj, &capability_attr_grp);
 	sysfs_remove_group(&intf->dev.kobj, &data_attr_grp);
+	usbtmc_free_int(data);
 	kref_put(&data->kref, usbtmc_delete);
 	return retcode;
 }
@@ -1196,6 +1554,7 @@ static void usbtmc_disconnect(struct usb_interface *intf)
 	dev_dbg(&intf->dev, "usbtmc_disconnect called\n");
 
 	data = usb_get_intfdata(intf);
+	usbtmc_free_int(data);
 	usb_deregister_dev(intf, &usbtmc_class);
 	sysfs_remove_group(&intf->dev.kobj, &capability_attr_grp);
 	sysfs_remove_group(&intf->dev.kobj, &data_attr_grp);
diff --git a/include/uapi/linux/usb/tmc.h b/include/uapi/linux/usb/tmc.h
index c045ae1..ac91c06 100644
--- a/include/uapi/linux/usb/tmc.h
+++ b/include/uapi/linux/usb/tmc.h
@@ -2,12 +2,14 @@
  * Copyright (C) 2007 Stefan Kopp, Gechingen, Germany
  * Copyright (C) 2008 Novell, Inc.
  * Copyright (C) 2008 Greg Kroah-Hartman <gregkh@suse.de>
+ * Copyright (C) 2015 Dave Penkler <dpenkler@gmail.com>
  *
  * This file holds USB constants defined by the USB Device Class
- * Definition for Test and Measurement devices published by the USB-IF.
+ * and USB488 Subclass Definitions for Test and Measurement devices
+ * published by the USB-IF.
  *
- * It also has the ioctl definitions for the usbtmc kernel driver that
- * userspace needs to know about.
+ * It also has the ioctl and capability definitions for the
+ * usbtmc kernel driver that userspace needs to know about.
  */
 
 #ifndef __LINUX_USB_TMC_H
@@ -30,14 +32,34 @@
 #define USBTMC_REQUEST_CHECK_CLEAR_STATUS		6
 #define USBTMC_REQUEST_GET_CAPABILITIES			7
 #define USBTMC_REQUEST_INDICATOR_PULSE			64
+#define USBTMC488_REQUEST_READ_STATUS_BYTE              128
+#define USBTMC488_REQUEST_REN_CONTROL                   160
+#define USBTMC488_REQUEST_GOTO_LOCAL                    161
+#define USBTMC488_REQUEST_LOCAL_LOCKOUT                 162
 
 /* Request values for USBTMC driver's ioctl entry point */
-#define USBTMC_IOC_NR			91
-#define USBTMC_IOCTL_INDICATOR_PULSE	_IO(USBTMC_IOC_NR, 1)
-#define USBTMC_IOCTL_CLEAR		_IO(USBTMC_IOC_NR, 2)
-#define USBTMC_IOCTL_ABORT_BULK_OUT	_IO(USBTMC_IOC_NR, 3)
-#define USBTMC_IOCTL_ABORT_BULK_IN	_IO(USBTMC_IOC_NR, 4)
-#define USBTMC_IOCTL_CLEAR_OUT_HALT	_IO(USBTMC_IOC_NR, 6)
-#define USBTMC_IOCTL_CLEAR_IN_HALT	_IO(USBTMC_IOC_NR, 7)
+#define USBTMC_IOC_NR                    91
+#define USBTMC_IOCTL_INDICATOR_PULSE     _IO(USBTMC_IOC_NR, 1)
+#define USBTMC_IOCTL_CLEAR               _IO(USBTMC_IOC_NR, 2)
+#define USBTMC_IOCTL_ABORT_BULK_OUT      _IO(USBTMC_IOC_NR, 3)
+#define USBTMC_IOCTL_ABORT_BULK_IN       _IO(USBTMC_IOC_NR, 4)
+#define USBTMC_IOCTL_CLEAR_OUT_HALT      _IO(USBTMC_IOC_NR, 6)
+#define USBTMC_IOCTL_CLEAR_IN_HALT       _IO(USBTMC_IOC_NR, 7)
+#define USBTMC488_IOCTL_GET_CAPABILITIES _IO(USBTMC_IOC_NR, 17)
+#define USBTMC488_IOCTL_READ_STB         _IOR(USBTMC_IOC_NR, 18, unsigned char)
+#define USBTMC488_IOCTL_REN_CONTROL      _IOW(USBTMC_IOC_NR, 19, unsigned char)
+#define USBTMC488_IOCTL_GOTO_LOCAL       _IO(USBTMC_IOC_NR, 20)
+#define USBTMC488_IOCTL_LOCAL_LOCKOUT    _IO(USBTMC_IOC_NR, 21)
 
+/* Driver encoded usb488 capabilities */
+#define USBTMC488_CAPABILITY_TRIGGER         1
+#define USBTMC488_CAPABILITY_SIMPLE          2
+#define USBTMC488_CAPABILITY_REN_CONTROL     2
+#define USBTMC488_CAPABILITY_GOTO_LOCAL      2
+#define USBTMC488_CAPABILITY_LOCAL_LOCKOUT   2
+#define USBTMC488_CAPABILITY_488_DOT_2       4
+#define USBTMC488_CAPABILITY_DT1             16
+#define USBTMC488_CAPABILITY_RL1             32
+#define USBTMC488_CAPABILITY_SR1             64
+#define USBTMC488_CAPABILITY_FULL_SCPI       128
 #endif
-- 
2.5.1