1 files changed, 162 insertions, 71 deletions

diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index 9c370427d2e..103a6e9ee49 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -19,7 +19,8 @@
  * http://www.gnu.org/copyleft/gpl.html.
  */
 
-#include <linux/init.h>
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/fs.h>
@@ -119,7 +120,6 @@ static void usbtmc_delete(struct kref *kref)
 	struct usbtmc_device_data *data = to_usbtmc_data(kref);
 
 	usb_put_dev(data->usb_dev);
-	kfree(data);
 }
 
 static int usbtmc_open(struct inode *inode, struct file *filp)
@@ -130,10 +130,8 @@ static int usbtmc_open(struct inode *inode, struct file *filp)
 
 	intf = usb_find_interface(&usbtmc_driver, iminor(inode));
 	if (!intf) {
-		printk(KERN_ERR KBUILD_MODNAME
-		       ": can not find device for minor %d", iminor(inode));
-		retval = -ENODEV;
-		goto exit;
+		pr_err("can not find device for minor %d", iminor(inode));
+		return -ENODEV;
 	}
 
 	data = usb_get_intfdata(intf);
@@ -142,7 +140,6 @@ static int usbtmc_open(struct inode *inode, struct file *filp)
 	/* Store pointer in file structure's private data field */
 	filp->private_data = data;
 
-exit:
 	return retval;
 }
 
@@ -386,20 +383,23 @@ exit:
 static int send_request_dev_dep_msg_in(struct usbtmc_device_data *data, size_t transfer_size)
 {
 	int retval;
-	u8 buffer[USBTMC_HEADER_SIZE];
+	u8 *buffer;
 	int actual;
 
+	buffer = kmalloc(USBTMC_HEADER_SIZE, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
 	/* Setup IO buffer for REQUEST_DEV_DEP_MSG_IN message
 	 * Refer to class specs for details
 	 */
 	buffer[0] = 2;
 	buffer[1] = data->bTag;
-	buffer[2] = ~(data->bTag);
+	buffer[2] = ~data->bTag;
 	buffer[3] = 0; /* Reserved */
-	buffer[4] = (transfer_size) & 255;
-	buffer[5] = ((transfer_size) >> 8) & 255;
-	buffer[6] = ((transfer_size) >> 16) & 255;
-	buffer[7] = ((transfer_size) >> 24) & 255;
+	buffer[4] = transfer_size >> 0;
+	buffer[5] = transfer_size >> 8;
+	buffer[6] = transfer_size >> 16;
+	buffer[7] = transfer_size >> 24;
 	buffer[8] = data->TermCharEnabled * 2;
 	/* Use term character? */
 	buffer[9] = data->TermChar;
@@ -418,8 +418,9 @@ static int send_request_dev_dep_msg_in(struct usbtmc_device_data *data, size_t t
 	/* Increment bTag -- and increment again if zero */
 	data->bTag++;
 	if (!data->bTag)
-		(data->bTag)++;
+		data->bTag++;
 
+	kfree(buffer);
 	if (retval < 0) {
 		dev_err(&data->intf->dev, "usb_bulk_msg in send_request_dev_dep_msg_in() returned %d\n", retval);
 		return retval;
@@ -455,21 +456,39 @@ static ssize_t usbtmc_read(struct file *filp, char __user *buf,
 		goto exit;
 	}
 
+	if (data->rigol_quirk) {
+		dev_dbg(dev, "usb_bulk_msg_in: count(%zu)\n", count);
+
+		retval = send_request_dev_dep_msg_in(data, count);
+
+		if (retval < 0) {
+			if (data->auto_abort)
+				usbtmc_ioctl_abort_bulk_out(data);
+			goto exit;
+		}
+	}
+
+	/* Loop until we have fetched everything we requested */
 	remaining = count;
+	this_part = remaining;
 	done = 0;
 
 	while (remaining > 0) {
-		if (remaining > USBTMC_SIZE_IOBUFFER - 12 - 3)
-			this_part = USBTMC_SIZE_IOBUFFER - 12 - 3;
-		else
-			this_part = remaining;
+		if (!data->rigol_quirk) {
+			dev_dbg(dev, "usb_bulk_msg_in: remaining(%zu), count(%zu)\n", remaining, count);
+
+			if (remaining > USBTMC_SIZE_IOBUFFER - USBTMC_HEADER_SIZE - 3)
+				this_part = USBTMC_SIZE_IOBUFFER - USBTMC_HEADER_SIZE - 3;
+			else
+				this_part = remaining;
 
 			retval = send_request_dev_dep_msg_in(data, this_part);
-		if (retval < 0) {
+			if (retval < 0) {
 			dev_err(dev, "usb_bulk_msg returned %d\n", retval);
-			if (data->auto_abort)
-				usbtmc_ioctl_abort_bulk_out(data);
-			goto exit;
+				if (data->auto_abort)
+					usbtmc_ioctl_abort_bulk_out(data);
+				goto exit;
+			}
 		}
 
 		/* Send bulk URB */
@@ -479,51 +498,109 @@ static ssize_t usbtmc_read(struct file *filp, char __user *buf,
 				      buffer, USBTMC_SIZE_IOBUFFER, &actual,
 				      USBTMC_TIMEOUT);
 
+		dev_dbg(dev, "usb_bulk_msg: retval(%u), done(%zu), remaining(%zu), actual(%d)\n", retval, done, remaining, actual);
+
 		/* Store bTag (in case we need to abort) */
 		data->bTag_last_read = data->bTag;
 
 		if (retval < 0) {
-			dev_err(dev, "Unable to read data, error %d\n", retval);
+			dev_dbg(dev, "Unable to read data, error %d\n", retval);
 			if (data->auto_abort)
 				usbtmc_ioctl_abort_bulk_in(data);
 			goto exit;
 		}
 
-		/* How many characters did the instrument send? */
-		n_characters = buffer[4] +
-			       (buffer[5] << 8) +
-			       (buffer[6] << 16) +
-			       (buffer[7] << 24);
+		/* Parse header in first packet */
+		if ((done == 0) || !data->rigol_quirk) {
+			/* Sanity checks for the header */
+			if (actual < USBTMC_HEADER_SIZE) {
+				dev_err(dev, "Device sent too small first packet: %u < %u\n", actual, USBTMC_HEADER_SIZE);
+				if (data->auto_abort)
+					usbtmc_ioctl_abort_bulk_in(data);
+				goto exit;
+			}
 
-		/* Ensure the instrument doesn't lie about it */
-		if(n_characters > actual - 12) {
-			dev_err(dev, "Device lies about message size: %u > %d\n", n_characters, actual - 12);
-			n_characters = actual - 12;
-		}
+			if (buffer[0] != 2) {
+				dev_err(dev, "Device sent reply with wrong MsgID: %u != 2\n", buffer[0]);
+				if (data->auto_abort)
+					usbtmc_ioctl_abort_bulk_in(data);
+				goto exit;
+			}
 
-		/* Ensure the instrument doesn't send more back than requested */
-		if(n_characters > this_part) {
-			dev_err(dev, "Device returns more than requested: %zu > %zu\n", done + n_characters, done + this_part);
-			n_characters = this_part;
-		}
+			if (buffer[1] != data->bTag_last_write) {
+				dev_err(dev, "Device sent reply with wrong bTag: %u != %u\n", buffer[1], data->bTag_last_write);
+				if (data->auto_abort)
+					usbtmc_ioctl_abort_bulk_in(data);
+				goto exit;
+			}
 
-		/* Bound amount of data received by amount of data requested */
-		if (n_characters > this_part)
-			n_characters = this_part;
+			/* How many characters did the instrument send? */
+			n_characters = buffer[4] +
+				       (buffer[5] << 8) +
+				       (buffer[6] << 16) +
+				       (buffer[7] << 24);
 
-		/* Copy buffer to user space */
-		if (copy_to_user(buf + done, &buffer[12], n_characters)) {
-			/* There must have been an addressing problem */
-			retval = -EFAULT;
-			goto exit;
+			if (n_characters > this_part) {
+				dev_err(dev, "Device wants to return more data than requested: %u > %zu\n", n_characters, count);
+				if (data->auto_abort)
+					usbtmc_ioctl_abort_bulk_in(data);
+				goto exit;
+			}
+
+			/* Remove the USBTMC header */
+			actual -= USBTMC_HEADER_SIZE;
+
+			/* Check if the message is smaller than requested */
+			if (data->rigol_quirk) {
+				if (remaining > n_characters)
+					remaining = n_characters;
+				/* Remove padding if it exists */
+				if (actual > remaining)
+					actual = remaining;
+			}
+			else {
+				if (this_part > n_characters)
+					this_part = n_characters;
+				/* Remove padding if it exists */
+				if (actual > this_part)
+					actual = this_part;
+			}
+
+			dev_dbg(dev, "Bulk-IN header: N_characters(%u), bTransAttr(%u)\n", n_characters, buffer[8]);
+
+			remaining -= actual;
+
+			/* Terminate if end-of-message bit received from device */
+			if ((buffer[8] & 0x01) && (actual >= n_characters))
+				remaining = 0;
+
+			dev_dbg(dev, "Bulk-IN header: remaining(%zu), buf(%p), buffer(%p) done(%zu)\n", remaining,buf,buffer,done);
+
+
+			/* Copy buffer to user space */
+			if (copy_to_user(buf + done, &buffer[USBTMC_HEADER_SIZE], actual)) {
+				/* There must have been an addressing problem */
+				retval = -EFAULT;
+				goto exit;
+			}
+			done += actual;
 		}
+		else  {
+			if (actual > remaining)
+				actual = remaining;
+
+			remaining -= actual;
+
+			dev_dbg(dev, "Bulk-IN header cont: actual(%u), done(%zu), remaining(%zu), buf(%p), buffer(%p)\n", actual, done, remaining,buf,buffer);
 
-		done += n_characters;
-		/* Terminate if end-of-message bit received from device */
-		if ((buffer[8] &  0x01) && (actual >= n_characters + 12))
-			remaining = 0;
-		else
-			remaining -= n_characters;
+			/* Copy buffer to user space */
+			if (copy_to_user(buf + done, buffer, actual)) {
+				/* There must have been an addressing problem */
+				retval = -EFAULT;
+				goto exit;
+			}
+			done += actual;
+		}
 	}
 
 	/* Update file position value */
@@ -564,8 +641,8 @@ static ssize_t usbtmc_write(struct file *filp, const char __user *buf,
 	done = 0;
 
 	while (remaining > 0) {
-		if (remaining > USBTMC_SIZE_IOBUFFER - 12) {
-			this_part = USBTMC_SIZE_IOBUFFER - 12;
+		if (remaining > USBTMC_SIZE_IOBUFFER - USBTMC_HEADER_SIZE) {
+			this_part = USBTMC_SIZE_IOBUFFER - USBTMC_HEADER_SIZE;
 			buffer[8] = 0;
 		} else {
 			this_part = remaining;
@@ -575,24 +652,24 @@ static ssize_t usbtmc_write(struct file *filp, const char __user *buf,
 		/* Setup IO buffer for DEV_DEP_MSG_OUT message */
 		buffer[0] = 1;
 		buffer[1] = data->bTag;
-		buffer[2] = ~(data->bTag);
+		buffer[2] = ~data->bTag;
 		buffer[3] = 0; /* Reserved */
-		buffer[4] = this_part & 255;
-		buffer[5] = (this_part >> 8) & 255;
-		buffer[6] = (this_part >> 16) & 255;
-		buffer[7] = (this_part >> 24) & 255;
+		buffer[4] = this_part >> 0;
+		buffer[5] = this_part >> 8;
+		buffer[6] = this_part >> 16;
+		buffer[7] = this_part >> 24;
 		/* buffer[8] is set above... */
 		buffer[9] = 0; /* Reserved */
 		buffer[10] = 0; /* Reserved */
 		buffer[11] = 0; /* Reserved */
 
-		if (copy_from_user(&buffer[12], buf + done, this_part)) {
+		if (copy_from_user(&buffer[USBTMC_HEADER_SIZE], buf + done, this_part)) {
 			retval = -EFAULT;
 			goto exit;
 		}
 
-		n_bytes = roundup(12 + this_part, 4);
-		memset(buffer + 12 + this_part, 0, n_bytes - (12 + this_part));
+		n_bytes = roundup(USBTMC_HEADER_SIZE + this_part, 4);
+		memset(buffer + USBTMC_HEADER_SIZE + this_part, 0, n_bytes - (USBTMC_HEADER_SIZE + this_part));
 
 		do {
 			retval = usb_bulk_msg(data->usb_dev,
@@ -825,7 +902,7 @@ err_out:
 }
 
 #define capability_attribute(name)					\
-static ssize_t show_##name(struct device *dev,				\
+static ssize_t name##_show(struct device *dev,				\
 			   struct device_attribute *attr, char *buf)	\
 {									\
 	struct usb_interface *intf = to_usb_interface(dev);		\
@@ -833,7 +910,7 @@ static ssize_t show_##name(struct device *dev,				\
 									\
 	return sprintf(buf, "%d\n", data->capabilities.name);		\
 }									\
-static DEVICE_ATTR(name, S_IRUGO, show_##name, NULL)
+static DEVICE_ATTR_RO(name)
 
 capability_attribute(interface_capabilities);
 capability_attribute(device_capabilities);
@@ -852,7 +929,7 @@ static struct attribute_group capability_attr_grp = {
 	.attrs = capability_attrs,
 };
 
-static ssize_t show_TermChar(struct device *dev,
+static ssize_t TermChar_show(struct device *dev,
 			     struct device_attribute *attr, char *buf)
 {
 	struct usb_interface *intf = to_usb_interface(dev);
@@ -861,7 +938,7 @@ static ssize_t show_TermChar(struct device *dev,
 	return sprintf(buf, "%c\n", data->TermChar);
 }
 
-static ssize_t store_TermChar(struct device *dev,
+static ssize_t TermChar_store(struct device *dev,
 			      struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
@@ -873,10 +950,10 @@ static ssize_t store_TermChar(struct device *dev,
 	data->TermChar = buf[0];
 	return count;
 }
-static DEVICE_ATTR(TermChar, S_IRUGO, show_TermChar, store_TermChar);
+static DEVICE_ATTR_RW(TermChar);
 
 #define data_attribute(name)						\
-static ssize_t show_##name(struct device *dev,				\
+static ssize_t name##_show(struct device *dev,				\
 			   struct device_attribute *attr, char *buf)	\
 {									\
 	struct usb_interface *intf = to_usb_interface(dev);		\
@@ -884,7 +961,7 @@ static ssize_t show_##name(struct device *dev,				\
 									\
 	return sprintf(buf, "%d\n", data->name);			\
 }									\
-static ssize_t store_##name(struct device *dev,				\
+static ssize_t name##_store(struct device *dev,				\
 			    struct device_attribute *attr,		\
 			    const char *buf, size_t count)		\
 {									\
@@ -902,7 +979,7 @@ static ssize_t store_##name(struct device *dev,				\
 	else								\
 		return count;						\
 }									\
-static DEVICE_ATTR(name, S_IRUGO, show_##name, store_##name)
+static DEVICE_ATTR_RW(name)
 
 data_attribute(TermCharEnabled);
 data_attribute(auto_abort);
@@ -1026,7 +1103,7 @@ static int usbtmc_probe(struct usb_interface *intf,
 
 	dev_dbg(&intf->dev, "%s called\n", __func__);
 
-	data = kmalloc(sizeof(struct usbtmc_device_data), GFP_KERNEL);
+	data = devm_kzalloc(&intf->dev, sizeof(*data), GFP_KERNEL);
 	if (!data) {
 		dev_err(&intf->dev, "Unable to allocate kernel memory\n");
 		return -ENOMEM;
@@ -1040,6 +1117,20 @@ static int usbtmc_probe(struct usb_interface *intf,
 	mutex_init(&data->io_mutex);
 	data->zombie = 0;
 
+	/* Determine if it is a Rigol or not */
+	data->rigol_quirk = 0;
+	dev_dbg(&intf->dev, "Trying to find if device Vendor 0x%04X Product 0x%04X has the RIGOL quirk\n",
+		le16_to_cpu(data->usb_dev->descriptor.idVendor),
+		le16_to_cpu(data->usb_dev->descriptor.idProduct));
+	for(n = 0; usbtmc_id_quirk[n].idVendor > 0; n++) {
+		if ((usbtmc_id_quirk[n].idVendor == le16_to_cpu(data->usb_dev->descriptor.idVendor)) &&
+		    (usbtmc_id_quirk[n].idProduct == le16_to_cpu(data->usb_dev->descriptor.idProduct))) {
+			dev_dbg(&intf->dev, "Setting this device as having the RIGOL quirk\n");
+			data->rigol_quirk = 1;
+			break;
+		}
+	}
+
 	/* Initialize USBTMC bTag and other fields */
 	data->bTag	= 1;
 	data->TermCharEnabled = 0;