staging: comedi: jr3_pci: tidy up jr3_download_firmware()

This callback function for comedi_load_firmware() first validates that
the firmware data is the correct format then it writes the data to each
subdevice.

Split the two operations out as separate functions to clarify the code.
Tidy up the new functions.
Signed-off-by: H Hartley Sweeten <hsweeten@visionengravers.com>
Cc: Ian Abbott <abbotti@mev.co.uk>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

drivers/staging/comedi/drivers/jr3_pci.c

@@ -326,105 +326,112 @@ static int read_idm_word(const u8 *data, size_t size, int *pos,
 	return result;
 }
 
-static int jr3_download_firmware(struct comedi_device *dev,
-				 const u8 *data, size_t size,
-				 unsigned long context)
+static int jr3_check_firmware(struct comedi_device *dev,
+			      const u8 *data, size_t size)
 {
+	int more = 1;
+	int pos = 0;
+
 	/*
 	 * IDM file format is:
 	 *   { count, address, data <count> } *
 	 *   ffff
 	 */
-	int result, more, pos, OK;
-
-	result = 0;
-	more = 1;
-	pos = 0;
-	OK = 0;
 	while (more) {
-		unsigned int count, addr;
+		unsigned int count = 0;
+		unsigned int addr = 0;
 
 		more = more && read_idm_word(data, size, &pos, &count);
-		if (more && count == 0xffff) {
-			OK = 1;
-			break;
-		}
+		if (more && count == 0xffff)
+			return 0;
+
 		more = more && read_idm_word(data, size, &pos, &addr);
 		while (more && count > 0) {
-			unsigned int dummy;
+			unsigned int dummy = 0;
+
 			more = more && read_idm_word(data, size, &pos, &dummy);
 			count--;
 		}
 	}
 
-	if (!OK) {
-		result = -ENODATA;
-	} else {
-		int i;
-		struct jr3_pci_dev_private *p = dev->private;
+	return -ENODATA;
+}
+
+static void jr3_write_firmware(struct comedi_device *dev,
+			       int subdev, const u8 *data, size_t size)
+{
+	struct jr3_pci_dev_private *devpriv = dev->private;
+	struct jr3_t __iomem *iobase = devpriv->iobase;
+	u32 __iomem *lo;
+	u32 __iomem *hi;
+	int more = 1;
+	int pos = 0;
 
-		for (i = 0; i < p->n_channels; i++) {
-			struct jr3_pci_subdev_private *sp;
+	while (more) {
+		unsigned int count = 0;
+		unsigned int addr = 0;
+
+		more = more && read_idm_word(data, size, &pos, &count);
+		if (more && count == 0xffff)
+			return;
+
+		more = more && read_idm_word(data, size, &pos, &addr);
+
+		dev_dbg(dev->class_dev, "Loading#%d %4.4x bytes at %4.4x\n",
+			subdev, count, addr);
+
+		while (more && count > 0) {
+			if (addr & 0x4000) {
+				/* 16 bit data, never seen in real life!! */
+				unsigned int data1 = 0;
+
+				more = more &&
+				       read_idm_word(data, size, &pos, &data1);
+				count--;
+				/* jr3[addr + 0x20000 * pnum] = data1; */
+			} else {
+				/* Download 24 bit program */
+				unsigned int data1 = 0;
+				unsigned int data2 = 0;
+
+				lo = &iobase->channel[subdev].program_lo[addr];
+				hi = &iobase->channel[subdev].program_hi[addr];
 
-			sp = dev->subdevices[i].private;
-			more = 1;
-			pos = 0;
-			while (more) {
-				unsigned int count, addr;
 				more = more &&
-				       read_idm_word(data, size, &pos, &count);
-				if (more && count == 0xffff)
-					break;
+				       read_idm_word(data, size, &pos, &data1);
 				more = more &&
-				       read_idm_word(data, size, &pos, &addr);
-				dev_dbg(dev->class_dev,
-					"Loading#%d %4.4x bytes at %4.4x\n",
-					i, count, addr);
-				while (more && count > 0) {
-					if (addr & 0x4000) {
-						/*  16 bit data, never seen
-						 *  in real life!! */
-						unsigned int data1;
-
-						more = more &&
-						       read_idm_word(data,
-								     size, &pos,
-								     &data1);
-						count--;
-						/* jr3[addr + 0x20000 * pnum] =
-						   data1; */
-					} else {
-						/*   Download 24 bit program */
-						unsigned int data1, data2;
-
-						more = more &&
-						       read_idm_word(data,
-								     size, &pos,
-								     &data1);
-						more = more &&
-						       read_idm_word(data, size,
-								     &pos,
-								     &data2);
-						count -= 2;
-						if (more) {
-							set_u16(&p->
-								iobase->channel
-								[i].program_low
-								[addr], data1);
-							udelay(1);
-							set_u16(&p->
-								iobase->channel
-								[i].program_high
-								[addr], data2);
-							udelay(1);
-						}
-					}
-					addr++;
+				       read_idm_word(data, size, &pos, &data2);
+				count -= 2;
+				if (more) {
+					set_u16(lo, data1);
+					udelay(1);
+					set_u16(hi, data2);
+					udelay(1);
 				}
 			}
+			addr++;
 		}
 	}
-	return result;
+}
+
+static int jr3_download_firmware(struct comedi_device *dev,
+				 const u8 *data, size_t size,
+				 unsigned long context)
+{
+	struct jr3_pci_dev_private *devpriv = dev->private;
+	int subdev;
+	int ret;
+
+	/* verify IDM file format */
+	ret = jr3_check_firmware(dev, data, size);
+	if (ret)
+		return ret;
+
+	/* write firmware to each subdevice */
+	for (subdev = 0; subdev < devpriv->n_channels; subdev++)
+		jr3_write_firmware(dev, subdev, data, size);
+
+	return 0;
 }
 
 static struct poll_delay_t jr3_pci_poll_subdevice(struct comedi_subdevice *s)

drivers/staging/comedi/drivers/jr3_pci.h

@@ -671,11 +671,11 @@ struct jr3_channel {
 
 struct jr3_t {
 	struct {
-		u32 program_low[0x4000];	/*  0x00000 - 0x10000 */
+		u32 program_lo[0x4000];		/*  0x00000 - 0x10000 */
 		struct jr3_channel data;	/*  0x10000 - 0x10c00 */
 		char pad2[0x30000 - 0x00c00];	/*  0x10c00 - 0x40000 */
-		u32 program_high[0x8000];	/*  0x40000 - 0x60000 */
-		u32 reset;	/*  0x60000 - 0x60004 */
+		u32 program_hi[0x8000];		/*  0x40000 - 0x60000 */
+		u32 reset;			/*  0x60000 - 0x60004 */
 		char pad3[0x20000 - 0x00004];	/*  0x60004 - 0x80000 */
 	} channel[4];
 };