staging: comedi: drivers: use comedi_dio_update_state() for complex cases

Use comedi_dio_update_state() to handle the boilerplate code to update
the subdevice s->state for more complex cases where the hardware is only
updated based on the 'mask' of the channels that are modified.

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>

9 files changed, 124 insertions, 141 deletions

diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 5283bb520a1d..5737bb4c29ab 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -126,30 +126,24 @@ EXPORT_SYMBOL_GPL(subdev_8255_interrupt);
 
 static int subdev_8255_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+			    struct comedi_insn *insn,
+			    unsigned int *data)
 {
 	struct subdev_8255_private *spriv = s->private;
 	unsigned long iobase = spriv->iobase;
 	unsigned int mask;
-	unsigned int bits;
 	unsigned int v;
 
-	mask = data[0];
-	bits = data[1];
-
+	mask = comedi_dio_update_state(s, data);
 	if (mask) {
-		v = s->state;
-		v &= ~mask;
-		v |= (bits & mask);
-
 		if (mask & 0xff)
-			spriv->io(1, _8255_DATA, v & 0xff, iobase);
+			spriv->io(1, _8255_DATA, s->state & 0xff, iobase);
 		if (mask & 0xff00)
-			spriv->io(1, _8255_DATA + 1, (v >> 8) & 0xff, iobase);
+			spriv->io(1, _8255_DATA + 1, (s->state >> 8) & 0xff,
+				  iobase);
 		if (mask & 0xff0000)
-			spriv->io(1, _8255_DATA + 2, (v >> 16) & 0xff, iobase);
-
-		s->state = v;
+			spriv->io(1, _8255_DATA + 2, (s->state >> 16) & 0xff,
+				  iobase);
 	}
 
 	v = spriv->io(0, _8255_DATA, 0, iobase);
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index 8c6fa1e62abf..2e4bf284d52c 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -941,31 +941,34 @@ static void dio200_subdev_8255_set_dir(struct comedi_device *dev,
 	dio200_write8(dev, subpriv->ofs + 3, config);
 }
 
-/*
- * Handle 'insn_bits' for an '8255' DIO subdevice.
- */
 static int dio200_subdev_8255_bits(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct dio200_subdev_8255 *subpriv = s->private;
+	unsigned int mask;
+	unsigned int val;
 
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-		if (data[0] & 0xff)
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		if (mask & 0xff)
 			dio200_write8(dev, subpriv->ofs, s->state & 0xff);
-		if (data[0] & 0xff00)
+		if (mask & 0xff00)
 			dio200_write8(dev, subpriv->ofs + 1,
 				      (s->state >> 8) & 0xff);
-		if (data[0] & 0xff0000)
+		if (mask & 0xff0000)
 			dio200_write8(dev, subpriv->ofs + 2,
 				      (s->state >> 16) & 0xff);
 	}
-	data[1] = dio200_read8(dev, subpriv->ofs);
-	data[1] |= dio200_read8(dev, subpriv->ofs + 1) << 8;
-	data[1] |= dio200_read8(dev, subpriv->ofs + 2) << 16;
-	return 2;
+
+	val = dio200_read8(dev, subpriv->ofs);
+	val |= dio200_read8(dev, subpriv->ofs + 1) << 8;
+	val |= dio200_read8(dev, subpriv->ofs + 2) << 16;
+
+	data[1] = val;
+
+	return insn->n;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 118a4fd129f9..b04a5633f754 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -596,52 +596,40 @@ static int dmm32at_ao_rinsn(struct comedi_device *dev,
 
 static int dmm32at_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
 	struct dmm32at_private *devpriv = dev->private;
-	unsigned char diobits;
-
-	/* The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit. */
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
-		/* Write out the new digital output lines */
-		/* outw(s->state,dev->iobase + DMM32AT_DIO); */
+	unsigned int mask;
+	unsigned int val;
+
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		/* get access to the DIO regs */
+		outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
+
+		/* if either part of dio is set for output */
+		if (((devpriv->dio_config & DMM32AT_DIRCL) == 0) ||
+		    ((devpriv->dio_config & DMM32AT_DIRCH) == 0)) {
+			val = (s->state & 0x00ff0000) >> 16;
+			outb(val, dev->iobase + DMM32AT_DIOC);
+		}
+		if ((devpriv->dio_config & DMM32AT_DIRB) == 0) {
+			val = (s->state & 0x0000ff00) >> 8;
+			outb(val, dev->iobase + DMM32AT_DIOB);
+		}
+		if ((devpriv->dio_config & DMM32AT_DIRA) == 0) {
+			val = (s->state & 0x000000ff);
+			outb(val, dev->iobase + DMM32AT_DIOA);
+		}
 	}
 
-	/* get access to the DIO regs */
-	outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
-
-	/* if either part of dio is set for output */
-	if (((devpriv->dio_config & DMM32AT_DIRCL) == 0) ||
-	    ((devpriv->dio_config & DMM32AT_DIRCH) == 0)) {
-		diobits = (s->state & 0x00ff0000) >> 16;
-		outb(diobits, dev->iobase + DMM32AT_DIOC);
-	}
-	if ((devpriv->dio_config & DMM32AT_DIRB) == 0) {
-		diobits = (s->state & 0x0000ff00) >> 8;
-		outb(diobits, dev->iobase + DMM32AT_DIOB);
-	}
-	if ((devpriv->dio_config & DMM32AT_DIRA) == 0) {
-		diobits = (s->state & 0x000000ff);
-		outb(diobits, dev->iobase + DMM32AT_DIOA);
-	}
+	val = inb(dev->iobase + DMM32AT_DIOA);
+	val |= inb(dev->iobase + DMM32AT_DIOB) << 8;
+	val |= inb(dev->iobase + DMM32AT_DIOC) << 16;
+	s->state = val;
 
-	/* now read the state back in */
-	s->state = inb(dev->iobase + DMM32AT_DIOC);
-	s->state <<= 8;
-	s->state |= inb(dev->iobase + DMM32AT_DIOB);
-	s->state <<= 8;
-	s->state |= inb(dev->iobase + DMM32AT_DIOA);
-	data[1] = s->state;
-
-	/* on return, data[1] contains the value of the digital
-	 * input and output lines. */
-	/* data[1]=inw(dev->iobase + DMM32AT_DIO); */
-	/* or we could just return the software copy of the output values if
-	 * it was a purely digital output subdevice */
-	/* data[1]=s->state; */
+	data[1] = val;
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/dt2817.c b/drivers/staging/comedi/drivers/dt2817.c
index f4a8529239b5..bf589936e546 100644
--- a/drivers/staging/comedi/drivers/dt2817.c
+++ b/drivers/staging/comedi/drivers/dt2817.c
@@ -80,36 +80,31 @@ static int dt2817_dio_insn_config(struct comedi_device *dev,
 
 static int dt2817_dio_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	unsigned int changed;
-
-	/* It's questionable whether it is more important in
-	 * a driver like this to be deterministic or fast.
-	 * We choose fast. */
-
-	if (data[0]) {
-		changed = s->state;
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-		changed ^= s->state;
-		changed &= s->io_bits;
-		if (changed & 0x000000ff)
-			outb(s->state & 0xff, dev->iobase + DT2817_DATA + 0);
-		if (changed & 0x0000ff00)
-			outb((s->state >> 8) & 0xff,
-			     dev->iobase + DT2817_DATA + 1);
-		if (changed & 0x00ff0000)
-			outb((s->state >> 16) & 0xff,
-			     dev->iobase + DT2817_DATA + 2);
-		if (changed & 0xff000000)
-			outb((s->state >> 24) & 0xff,
-			     dev->iobase + DT2817_DATA + 3);
+	unsigned long iobase = dev->iobase + DT2817_DATA;
+	unsigned int mask;
+	unsigned int val;
+
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		if (mask & 0x000000ff)
+			outb(s->state & 0xff, iobase + 0);
+		if (mask & 0x0000ff00)
+			outb((s->state >> 8) & 0xff, iobase + 1);
+		if (mask & 0x00ff0000)
+			outb((s->state >> 16) & 0xff, iobase + 2);
+		if (mask & 0xff000000)
+			outb((s->state >> 24) & 0xff, iobase + 3);
 	}
-	data[1] = inb(dev->iobase + DT2817_DATA + 0);
-	data[1] |= (inb(dev->iobase + DT2817_DATA + 1) << 8);
-	data[1] |= (inb(dev->iobase + DT2817_DATA + 2) << 16);
-	data[1] |= (inb(dev->iobase + DT2817_DATA + 3) << 24);
+
+	val = inb(iobase + 0);
+	val |= (inb(iobase + 1) << 8);
+	val |= (inb(iobase + 2) << 16);
+	val |= (inb(iobase + 3) << 24);
+
+	data[1] = val;
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index c2745f201f2c..44557c9671ac 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -163,29 +163,29 @@ static int ni6527_di_insn_bits(struct comedi_device *dev,
 
 static int ni6527_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
 	struct ni6527_private *devpriv = dev->private;
+	unsigned int mask;
 
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-
-		/* The open relay state on the board cooresponds to 1,
-		 * but in Comedi, it is represented by 0. */
-		if (data[0] & 0x0000ff) {
-			writeb((s->state ^ 0xff),
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		/* Outputs are inverted */
+		if (mask & 0x0000ff) {
+			writeb(s->state ^ 0xff,
 			       devpriv->mite->daq_io_addr + Port_Register(3));
 		}
-		if (data[0] & 0x00ff00) {
+		if (mask & 0x00ff00) {
 			writeb((s->state >> 8) ^ 0xff,
 			       devpriv->mite->daq_io_addr + Port_Register(4));
 		}
-		if (data[0] & 0xff0000) {
+		if (mask & 0xff0000) {
 			writeb((s->state >> 16) ^ 0xff,
 			       devpriv->mite->daq_io_addr + Port_Register(5));
 		}
 	}
+
 	data[1] = s->state;
 
 	return insn->n;
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index 0ac875f18787..e4cdca349157 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -72,18 +72,22 @@ Manuals:	Register level:	http://www.ni.com/pdf/manuals/340698.pdf
 
 static int daq700_dio_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
+	unsigned int mask;
+	unsigned int val;
 
-		if (data[0] & 0xff)
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		if (mask & 0xff)
 			outb(s->state & 0xff, dev->iobase + DIO_W);
 	}
 
-	data[1] = s->state & 0xff;
-	data[1] |= inb(dev->iobase + DIO_R) << 8;
+	val = s->state & 0xff;
+	val |= inb(dev->iobase + DIO_R) << 8;
+
+	data[1] = val;
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index e859f85a8e17..53cd1af7ce9d 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -422,19 +422,20 @@ static int pcl711_di_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-/* Digital port write - Untested on 8112 */
 static int pcl711_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
+	unsigned int mask;
+
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		if (mask & 0x00ff)
+			outb(s->state & 0xff, dev->iobase + PCL711_DO_LO);
+		if (mask & 0xff00)
+			outb((s->state >> 8), dev->iobase + PCL711_DO_HI);
 	}
-	if (data[0] & 0x00ff)
-		outb(s->state & 0xff, dev->iobase + PCL711_DO_LO);
-	if (data[0] & 0xff00)
-		outb((s->state >> 8), dev->iobase + PCL711_DO_HI);
 
 	data[1] = s->state;
 
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index a4d0bcc31e52..32fb57a7fa26 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -196,18 +196,19 @@ static int pcl726_di_insn_bits(struct comedi_device *dev,
 
 static int pcl726_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
 	const struct pcl726_board *board = comedi_board(dev);
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
+	unsigned int mask;
+
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		if (mask & 0x00ff)
+			outb(s->state & 0xff, dev->iobase + board->do_lo);
+		if (mask & 0xff00)
+			outb((s->state >> 8), dev->iobase + board->do_hi);
 	}
-	if (data[1] & 0x00ff)
-		outb(s->state & 0xff, dev->iobase + board->do_lo);
-	if (data[1] & 0xff00)
-		outb((s->state >> 8), dev->iobase + board->do_hi);
 
 	data[1] = s->state;
 
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index 2a659f23ecda..d041b714db29 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -167,20 +167,17 @@ static int pcl730_do_insn_bits(struct comedi_device *dev,
 			       unsigned int *data)
 {
 	unsigned long reg = (unsigned long)s->private;
-	unsigned int mask = data[0];
-	unsigned int bits = data[1];
+	unsigned int mask;
 
+	mask = comedi_dio_update_state(s, data);
 	if (mask) {
-		s->state &= ~mask;
-		s->state |= (bits & mask);
-
 		if (mask & 0x00ff)
 			outb(s->state & 0xff, dev->iobase + reg);
-		if ((mask & 0xff00) && (s->n_chan > 8))
+		if ((mask & 0xff00) & (s->n_chan > 8))
 			outb((s->state >> 8) & 0xff, dev->iobase + reg + 1);
-		if ((mask & 0xff0000) && (s->n_chan > 16))
+		if ((mask & 0xff0000) & (s->n_chan > 16))
 			outb((s->state >> 16) & 0xff, dev->iobase + reg + 2);
-		if ((mask & 0xff000000) && (s->n_chan > 24))
+		if ((mask & 0xff000000) & (s->n_chan > 24))
 			outb((s->state >> 24) & 0xff, dev->iobase + reg + 3);
 	}