usb: usbtmc: Add support for missing functions in USBTMC-USB488 spec

index 7a11a82..89456df9 100644
@@ -87,6 +87,19 @@ 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;
+
 	u8 rigol_quirk;
 
 	/* attributes from the USB TMC spec for this device */

@@ -99,6 +112,7 @@ 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;
 };
 #define to_usbtmc_data(d) container_of(d, struct usbtmc_device_data, kref)
 

@@ -373,6 +387,83 @@ exit:
 	return rv;
 }
 
+static int usbtmc488_ioctl_read_stb(struct usbtmc_device_data *data,
+				unsigned long arg)
+{
+	struct device *dev = &data->intf->dev;
+	u8 *buffer;
+	u8 tag, stb;
+	int rv;
+
+	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);
+
+	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)
+		/* 1 is for SRQ see USBTMC-USB488 subclass spec section 4.3.1 */
+		data->iin_bTag = 2;
+
+	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.

@@ -1069,6 +1160,10 @@ 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_READ_STB:
+		retval = usbtmc488_ioctl_read_stb(data, arg);
+		break;
 	}
 
 skip_io_on_zombie:

@@ -1092,6 +1187,57 @@ 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;
+	struct device *dev = &data->intf->dev;
+	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 */
+		/* check for valid STB notification */
+		if (data->iin_buffer[0] > 0x81) {
+			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;
+		}
+		dev_warn(dev, "invalid notification: %x\n", data->iin_buffer[0]);
+		break;
+	case -EOVERFLOW:
+		dev_err(dev, "overflow with length %d, actual length is %d\n",
+			data->iin_wMaxPacketSize, urb->actual_length);
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -EILSEQ:
+	case -ETIME:
+		/* urb terminated, clean up */
+		dev_dbg(dev, "urb terminated, status: %d\n", status);
+		return;
+	default:
+		dev_err(dev, "unknown status received: %d\n", status);
+	}
+exit:
+	rv = usb_submit_urb(urb, GFP_ATOMIC);
+	if (rv)
+		dev_err(dev, "usb_submit_urb failed: %d\n", rv);
+}
+
+static void usbtmc_free_int(struct usbtmc_device_data *data)
+{
+	if (!data->iin_ep_present || !data->iin_urb)
+		return;
+	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 +1260,8 @@ 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);
 	data->zombie = 0;
 
 	/* Determine if it is a Rigol or not */

@@ -1134,9 +1282,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 +1312,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 +1334,39 @@ 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);
+
+		retcode = usb_submit_urb(data->iin_urb, GFP_KERNEL);
+		if (retcode) {
+			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 +1383,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 +1395,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);

index c045ae1..7e5ced8 100644
@@ -30,6 +30,7 @@
 #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
 
 /* Request values for USBTMC driver's ioctl entry point */
 #define USBTMC_IOC_NR			91

@@ -39,5 +40,6 @@
 #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_READ_STB	_IOR(USBTMC_IOC_NR, 18, unsigned char)
 
 #endif

index 89456df9..7bfd6ec 100644
@@ -99,6 +99,7 @@ struct usbtmc_device_data {
 	int            iin_interval;
 	struct urb    *iin_urb;
 	u16            iin_wMaxPacketSize;
+	atomic_t       srq_asserted;
 
 	u8 rigol_quirk;
 

@@ -113,6 +114,7 @@ struct usbtmc_device_data {
 	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)
 

@@ -404,6 +406,9 @@ static int usbtmc488_ioctl_read_stb(struct usbtmc_device_data *data,
 
 	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,

@@ -1171,6 +1176,13 @@ 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 const struct file_operations fops = {
 	.owner		= THIS_MODULE,
 	.read		= usbtmc_read,

@@ -1178,6 +1190,7 @@ static const struct file_operations fops = {
 	.open		= usbtmc_open,
 	.release	= usbtmc_release,
 	.unlocked_ioctl	= usbtmc_ioctl,
+	.fasync         = usbtmc_fasync,
 	.llseek		= default_llseek,
 };
 

@@ -1207,6 +1220,16 @@ static void usbtmc_interrupt(struct urb *urb)
 			wake_up_interruptible(&data->waitq);
 			goto exit;
 		}
+		/* check for SRQ notification */
+		if (data->iin_buffer[0] == 0x81) {
+			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(dev, "invalid notification: %x\n", data->iin_buffer[0]);
 		break;
 	case -EOVERFLOW:

@@ -1262,6 +1285,7 @@ static int usbtmc_probe(struct usb_interface *intf,
 	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 */

index 7bfd6ec..3b85ef5 100644
@@ -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>

@@ -1183,6 +1184,27 @@ static int usbtmc_fasync(int fd, struct file *file, int on)
 	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;
+
+	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,

@@ -1191,6 +1213,7 @@ static const struct file_operations fops = {
 	.release	= usbtmc_release,
 	.unlocked_ioctl	= usbtmc_ioctl,
 	.fasync         = usbtmc_fasync,
+	.poll           = usbtmc_poll,
 	.llseek		= default_llseek,
 };
 

index 3b85ef5..3a3264c 100644
@@ -102,6 +102,9 @@ struct usbtmc_device_data {
 	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 */

@@ -992,6 +995,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:

@@ -1167,6 +1171,14 @@ static long usbtmc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		retval = usbtmc_ioctl_abort_bulk_in(data);
 		break;
 
+	case USBTMC488_IOCTL_GET_CAPS:
+		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;

index 7e5ced8..1dc3af1 100644
@@ -2,12 +2,14 @@
  * Copyright (C) 2007 Stefan Kopp, Gechingen, Germany
  * Copyright (C) 2008 Novell, Inc.
  * Copyright (C) 2008 Greg Kroah-Hartman <[email protected]>
+ * Copyright (C) 2015 Dave Penkler <[email protected]>
  *
  * 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

@@ -40,6 +42,19 @@
 #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_CAPS	_IO(USBTMC_IOC_NR, 17)
 #define USBTMC488_IOCTL_READ_STB	_IOR(USBTMC_IOC_NR, 18, unsigned char)
 
+/* 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

index 3a3264c..f04a086 100644
@@ -473,6 +473,62 @@ static int usbtmc488_ioctl_read_stb(struct usbtmc_device_data *data,
 	return rv;
 }
 
+static int usbtmc488_ioctl_simple(struct usbtmc_device_data *data,
+				unsigned long arg,
+				unsigned int cmd)
+{
+	struct device *dev = &data->intf->dev;
+	unsigned int val;
+	u8 *buffer;
+	u16 wValue;
+	int rv;
+
+	if (!(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;
+	}
+	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.

@@ -1182,6 +1238,21 @@ static long usbtmc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	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:

index 1dc3af1..0d852c9 100644
@@ -33,6 +33,9 @@
 #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

@@ -44,6 +47,9 @@
 #define USBTMC_IOCTL_CLEAR_IN_HALT	_IO(USBTMC_IOC_NR, 7)
 #define USBTMC488_IOCTL_GET_CAPS	_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