staging: comedi: mpc624: introduce mpc624_ai_get_sample()

Introduce a helper function to read the analog sample from the serially
connected A/D converter and handle the munging of the data.

Signed-off-by: H Hartley Sweeten <hsweeten@visionengravers.com>
Reviewed-by: Ian Abbott <abbotti@mev.co.uk>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

1 files changed, 83 insertions, 89 deletions

diff --git a/drivers/staging/comedi/drivers/mpc624.c b/drivers/staging/comedi/drivers/mpc624.c
index 0e8976045bff..3763dc52e48e 100644
--- a/drivers/staging/comedi/drivers/mpc624.c
+++ b/drivers/staging/comedi/drivers/mpc624.c
@@ -115,6 +115,85 @@ static const struct comedi_lrange range_mpc624_bipolar10 = {
 	 }
 };
 
+static unsigned int mpc624_ai_get_sample(struct comedi_device *dev,
+					 struct comedi_subdevice *s)
+{
+	struct mpc624_private *devpriv = dev->private;
+	unsigned int data_out = devpriv->ai_speed;
+	unsigned int data_in = 0;
+	unsigned int bit;
+	int i;
+
+	/* Start reading data */
+	udelay(1);
+	for (i = 0; i < 32; i++) {
+		/* Set the clock low */
+		outb(0, dev->iobase + MPC624_ADC);
+		udelay(1);
+
+		/* Set the ADSDI line for the next bit (send to MPC624) */
+		bit = (data_out & BIT(31)) ? MPC624_ADSDI : 0;
+		outb(bit, dev->iobase + MPC624_ADC);
+		udelay(1);
+
+		/* Set the clock high */
+		outb(MPC624_ADSCK | bit, dev->iobase + MPC624_ADC);
+		udelay(1);
+
+		/* Read ADSDO on high clock (receive from MPC624) */
+		data_in <<= 1;
+		data_in |= (inb(dev->iobase + MPC624_ADC) & MPC624_ADSDO) >> 4;
+		udelay(1);
+
+		data_out <<= 1;
+	}
+
+	/*
+	 * Received 32-bit long value consist of:
+	 *	31: EOC - (End Of Transmission) bit - should be 0
+	 *	30: DMY - (Dummy) bit - should be 0
+	 *	29: SIG - (Sign) bit - 1 if positive, 0 if negative
+	 *	28: MSB - (Most Significant Bit) - the first bit of the
+	 *					   conversion result
+	 *	....
+	 *	05: LSB - (Least Significant Bit)- the last bit of the
+	 *					   conversion result
+	 *	04-00: sub-LSB - sub-LSBs are basically noise, but when
+	 *			 averaged properly, they can increase
+	 *			 conversion precision up to 29 bits;
+	 *			 they can be discarded without loss of
+	 *			 resolution.
+	 */
+	if (data_in & MPC624_EOC_BIT)
+		dev_dbg(dev->class_dev, "EOC bit is set!");
+	if (data_in & MPC624_DMY_BIT)
+		dev_dbg(dev->class_dev, "DMY bit is set!");
+
+	if (data_in & MPC624_SGN_BIT) {
+		/*
+		 * Voltage is positive
+		 *
+		 * comedi operates on unsigned numbers, so mask off EOC
+		 * and DMY and don't clear the SGN bit
+		 */
+		data_in &= 0x3fffffff;
+	} else {
+		/*
+		 * The voltage is negative
+		 *
+		 * data_in contains a number in 30-bit two's complement
+		 * code and we must deal with it
+		 */
+		data_in |= MPC624_SGN_BIT;
+		data_in = ~data_in;
+		data_in += 1;
+		/* clear EOC and DMY bits */
+		data_in &= ~(MPC624_EOC_BIT | MPC624_DMY_BIT);
+		data_in = 0x20000000 - data_in;
+	}
+	return data_in;
+}
+
 static int mpc624_ai_eoc(struct comedi_device *dev,
 			 struct comedi_subdevice *s,
 			 struct comedi_insn *insn,
@@ -133,10 +212,8 @@ static int mpc624_ai_insn_read(struct comedi_device *dev,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	struct mpc624_private *devpriv = dev->private;
-	int n, i;
-	unsigned long int data_in, data_out;
 	int ret;
+	int i;
 
 	/*
 	 *  WARNING:
@@ -144,7 +221,7 @@ static int mpc624_ai_insn_read(struct comedi_device *dev,
 	 */
 	outb(insn->chanspec, dev->iobase + MPC624_GNMUXCH);
 
-	for (n = 0; n < insn->n; n++) {
+	for (i = 0; i < insn->n; i++) {
 		/*  Trigger the conversion */
 		outb(MPC624_ADSCK, dev->iobase + MPC624_ADC);
 		udelay(1);
@@ -158,93 +235,10 @@ static int mpc624_ai_insn_read(struct comedi_device *dev,
 		if (ret)
 			return ret;
 
-		/*  Start reading data */
-		data_in = 0;
-		data_out = devpriv->ai_speed;
-		udelay(1);
-		for (i = 0; i < 32; i++) {
-			/*  Set the clock low */
-			outb(0, dev->iobase + MPC624_ADC);
-			udelay(1);
-
-			if (data_out & BIT(31)) { /*  the next bit is a 1 */
-				/*  Set the ADSDI line (send to MPC624) */
-				outb(MPC624_ADSDI, dev->iobase + MPC624_ADC);
-				udelay(1);
-				/*  Set the clock high */
-				outb(MPC624_ADSCK | MPC624_ADSDI,
-				     dev->iobase + MPC624_ADC);
-			} else {	/*  the next bit is a 0 */
-
-				/*  Set the ADSDI line (send to MPC624) */
-				outb(0, dev->iobase + MPC624_ADC);
-				udelay(1);
-				/*  Set the clock high */
-				outb(MPC624_ADSCK, dev->iobase + MPC624_ADC);
-			}
-			/*  Read ADSDO on high clock (receive from MPC624) */
-			udelay(1);
-			data_in <<= 1;
-			data_in |=
-			    (inb(dev->iobase + MPC624_ADC) & MPC624_ADSDO) >> 4;
-			udelay(1);
-
-			data_out <<= 1;
-		}
-
-		/*
-		 *  Received 32-bit long value consist of:
-		 *    31: EOC -
-		 *          (End Of Transmission) bit - should be 0
-		 *    30: DMY
-		 *          (Dummy) bit - should be 0
-		 *    29: SIG
-		 *          (Sign) bit- 1 if the voltage is positive,
-		 *                      0 if negative
-		 *    28: MSB
-		 *          (Most Significant Bit) - the first bit of
-		 *                                   the conversion result
-		 *    ....
-		 *    05: LSB
-		 *          (Least Significant Bit)- the last bit of the
-		 *                                   conversion result
-		 *    04-00: sub-LSB
-		 *          - sub-LSBs are basically noise, but when
-		 *            averaged properly, they can increase conversion
-		 *            precision up to 29 bits; they can be discarded
-		 *            without loss of resolution.
-		 */
-
-		if (data_in & MPC624_EOC_BIT)
-			dev_dbg(dev->class_dev,
-				"EOC bit is set (data_in=%lu)!", data_in);
-		if (data_in & MPC624_DMY_BIT)
-			dev_dbg(dev->class_dev,
-				"DMY bit is set (data_in=%lu)!", data_in);
-		if (data_in & MPC624_SGN_BIT) {	/* Volatge is positive */
-			/*
-			 * comedi operates on unsigned numbers, so mask off EOC
-			 * and DMY and don't clear the SGN bit
-			 */
-			data_in &= 0x3FFFFFFF;
-			data[n] = data_in;
-		} else { /*  The voltage is negative */
-			/*
-			 * data_in contains a number in 30-bit two's complement
-			 * code and we must deal with it
-			 */
-			data_in |= MPC624_SGN_BIT;
-			data_in = ~data_in;
-			data_in += 1;
-			data_in &= ~(MPC624_EOC_BIT | MPC624_DMY_BIT);
-			/*  clear EOC and DMY bits */
-			data_in = 0x20000000 - data_in;
-			data[n] = data_in;
-		}
+		data[i] = mpc624_ai_get_sample(dev, s);
 	}
 
-	/*  Return the number of samples read/written */
-	return n;
+	return insn->n;
 }
 
 static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)