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

Convert the drivers with complex, port programmable i/o, to use the
comedi_dio_insn_config() helper function.

All of these drivers have some sort of 'port' programmable i/o where multiple
i/o channels are configured as a group. The 'mask' associated with the group
is passed to comedi_dio_insn_config() so that all the channels are configured.

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

15 files changed, 263 insertions, 433 deletions

diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 1fa29ac4eb5d..2f070fdbbb1d 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -184,39 +184,29 @@ static void subdev_8255_do_config(struct comedi_device *dev,
 
 static int subdev_8255_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int mask;
-	unsigned int bits;
+	int ret;
 
-	mask = 1 << CR_CHAN(insn->chanspec);
-	if (mask & 0x0000ff)
-		bits = 0x0000ff;
-	else if (mask & 0x00ff00)
-		bits = 0x00ff00;
-	else if (mask & 0x0f0000)
-		bits = 0x0f0000;
+	if (chan < 8)
+		mask = 0x0000ff;
+	else if (chan < 16)
+		mask = 0x00ff00;
+	else if (chan < 20)
+		mask = 0x0f0000;
 	else
-		bits = 0xf00000;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-	}
+		mask = 0xf00000;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	subdev_8255_do_config(dev, s);
 
-	return 1;
+	return insn->n;
 }
 
 static int subdev_8255_cmdtest(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/addi_apci_16xx.c b/drivers/staging/comedi/drivers/addi_apci_16xx.c
index 43296a6e7d4e..96523744b8de 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -60,36 +60,22 @@ static int apci16xx_insn_config(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	unsigned int chan_mask = 1 << CR_CHAN(insn->chanspec);
-	unsigned int bits;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
 
-	/*
-	 * Each 8-bit "port" is configurable as either input or
-	 * output. Changing the configuration of any channel in
-	 * a port changes the entire port.
-	 */
-	if (chan_mask & 0x000000ff)
-		bits = 0x000000ff;
-	else if (chan_mask & 0x0000ff00)
-		bits = 0x0000ff00;
-	else if (chan_mask & 0x00ff0000)
-		bits = 0x00ff0000;
+	if (chan < 8)
+		mask = 0x000000ff;
+	else if (chan < 16)
+		mask = 0x0000ff00;
+	else if (chan < 24)
+		mask = 0x00ff0000;
 	else
-		bits = 0xff000000;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_INPUT : COMEDI_OUTPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
-	}
+		mask = 0xff000000;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	outl(s->io_bits, dev->iobase + APCI16XX_DIR_REG(s->index));
 
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index dbc0678027ca..cf5dd10eaf91 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -686,38 +686,28 @@ static int apci3xxx_dio_insn_config(struct comedi_device *dev,
 				    unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask = 1 << chan;
-	unsigned int bits;
+	unsigned int mask;
+	int ret;
 
 	/*
 	 * Port 0 (channels 0-7) are always inputs
 	 * Port 1 (channels 8-15) are always outputs
 	 * Port 2 (channels 16-23) are programmable i/o
-	 *
-	 * Changing any channel in port 2 changes the entire port.
 	 */
-	if (mask & 0xff0000)
-		bits = 0xff0000;
-	else
-		bits = 0;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
+	if (chan < 16) {
+		if (data[0] != INSN_CONFIG_DIO_QUERY)
+			return -EINVAL;
+	} else {
+		/* changing any channel in port 2 changes the entire port */
+		mask = 0xff0000;
 	}
 
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
+
 	/* update port 2 configuration */
-	if (bits)
-		outl((s->io_bits >> 24) & 0xff, dev->iobase + 224);
+	outl((s->io_bits >> 24) & 0xff, dev->iobase + 224);
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index f914fb1f571b..b793d6987b84 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -180,38 +180,29 @@ static int pci1723_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int mask;
-	unsigned int bits;
-	unsigned short dio_mode;
+	unsigned short mode;
+	int ret;
 
-	mask = 1 << CR_CHAN(insn->chanspec);
-	if (mask & 0x00FF)
-		bits = 0x00FF;
+	if (chan < 8)
+		mask = 0x00ff;
 	else
-		bits = 0xFF00;
+		mask = 0xff00;
 
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
-	}
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	/* update hardware DIO mode */
-	dio_mode = 0x0000;	/* low byte output, high byte output */
-	if ((s->io_bits & 0x00FF) == 0)
-		dio_mode |= 0x0001;	/* low byte input */
-	if ((s->io_bits & 0xFF00) == 0)
-		dio_mode |= 0x0002;	/* high byte input */
-	outw(dio_mode, dev->iobase + PCI1723_DIGITAL_IO_PORT_SET);
-	return 1;
+	mode = 0x0000;			/* assume output */
+	if (!(s->io_bits & 0x00ff))
+		mode |= 0x0001;		/* low byte input */
+	if (!(s->io_bits & 0xff00))
+		mode |= 0x0002;		/* high byte input */
+	outw(mode, dev->iobase + PCI1723_DIGITAL_IO_PORT_SET);
+
+	return insn->n;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index 32c490b27b55..c1f723e86146 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -976,34 +976,26 @@ static int dio200_subdev_8255_config(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int mask;
-	unsigned int bits;
-
-	mask = 1 << CR_CHAN(insn->chanspec);
-	if (mask & 0x0000ff)
-		bits = 0x0000ff;
-	else if (mask & 0x00ff00)
-		bits = 0x00ff00;
-	else if (mask & 0x0f0000)
-		bits = 0x0f0000;
+	int ret;
+
+	if (chan < 8)
+		mask = 0x0000ff;
+	else if (chan < 16)
+		mask = 0x00ff00;
+	else if (chan < 20)
+		mask = 0x0f0000;
 	else
-		bits = 0xf00000;
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-	}
+		mask = 0xf00000;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
+
 	dio200_subdev_8255_set_dir(dev, s);
-	return 1;
+
+	return insn->n;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 05e01a3b5af6..0ce93da70847 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -341,33 +341,22 @@ static int das16cs_dio_insn_bits(struct comedi_device *dev,
 
 static int das16cs_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct das16cs_private *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-	int bits;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
 
 	if (chan < 4)
-		bits = 0x0f;
+		mask = 0x0f;
 	else
-		bits = 0xf0;
+		mask = 0xf0;
 
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-		break;
-	}
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	devpriv->status2 &= ~0x00c0;
 	devpriv->status2 |= (s->io_bits & 0xf0) ? 0x0080 : 0;
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index e0e7beab7274..38918a1198aa 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -551,32 +551,19 @@ static int dt2801_dio_insn_bits(struct comedi_device *dev,
 
 static int dt2801_dio_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-	int which = 0;
+	int ret;
 
-	if (s == &dev->subdevices[3])
-		which = 1;
+	ret = comedi_dio_insn_config(dev, s, insn, data, 0xff);
+	if (ret)
+		return ret;
 
-	/* configure */
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits = 0xff;
-		dt2801_writecmd(dev, DT_C_SET_DIGOUT);
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits = 0;
-		dt2801_writecmd(dev, DT_C_SET_DIGIN);
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = s->io_bits ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
-	}
-	dt2801_writedata(dev, which);
+	dt2801_writecmd(dev, s->io_bits ? DT_C_SET_DIGOUT : DT_C_SET_DIGIN);
+	dt2801_writedata(dev, (s == &dev->subdevices[3]) ? 1 : 0);
 
-	return 1;
+	return insn->n;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index e4748da1993b..64ef87598b60 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -642,32 +642,23 @@ static void dt3k_dio_config(struct comedi_device *dev, int bits)
 
 static int dt3k_dio_insn_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	int mask;
-
-	mask = (CR_CHAN(insn->chanspec) < 4) ? 0x0f : 0xf0;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= mask;
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~mask;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->
-		     io_bits & (1 << CR_CHAN(insn->chanspec))) ? COMEDI_OUTPUT :
-		    COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-		break;
-	}
-	mask = (s->io_bits & 0x01) | ((s->io_bits & 0x10) >> 3);
-	dt3k_dio_config(dev, mask);
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
+
+	if (chan < 4)
+		mask = 0x0f;
+	else
+		mask = 0xf0;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
+
+	dt3k_dio_config(dev, (s->io_bits & 0x01) | ((s->io_bits & 0x10) >> 3));
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index cdcc8f42e209..559bf5583530 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -224,37 +224,26 @@ struct hpdi_private {
 	volatile uint32_t bits[24];
 	/* number of bytes at which to generate COMEDI_CB_BLOCK events */
 	volatile unsigned int block_size;
-	unsigned dio_config_output:1;
 };
 
 static int dio_config_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
+			   struct comedi_subdevice *s,
+			   struct comedi_insn *insn,
 			   unsigned int *data)
 {
-	struct hpdi_private *devpriv = dev->private;
+	int ret;
 
 	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		devpriv->dio_config_output = 1;
-		return insn->n;
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		devpriv->dio_config_output = 0;
-		return insn->n;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    devpriv->dio_config_output ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
 	case INSN_CONFIG_BLOCK_SIZE:
 		return dio_config_block_size(dev, data);
-		break;
 	default:
+		ret = comedi_dio_insn_config(dev, s, insn, data, 0xffffffff);
+		if (ret)
+			return ret;
 		break;
 	}
 
-	return -EINVAL;
+	return insn->n;
 }
 
 static void disable_plx_interrupts(struct comedi_device *dev)
@@ -673,9 +662,7 @@ static int di_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 static int hpdi_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_cmd *cmd)
 {
-	struct hpdi_private *devpriv = dev->private;
-
-	if (devpriv->dio_config_output)
+	if (s->io_bits)
 		return -EINVAL;
 	else
 		return di_cmd_test(dev, s, cmd);
@@ -746,9 +733,7 @@ static int di_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static int hpdi_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct hpdi_private *devpriv = dev->private;
-
-	if (devpriv->dio_config_output)
+	if (s->io_bits)
 		return -EINVAL;
 	else
 		return di_cmd(dev, s);
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index 954c5397f33c..5c3a318b4640 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -350,31 +350,22 @@ static int ii20k_dio_insn_config(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	unsigned int mask = 1 << CR_CHAN(insn->chanspec);
-	unsigned int bits;
-
-	if (mask & 0x000000ff)
-		bits = 0x000000ff;
-	else if (mask & 0x0000ff00)
-		bits = 0x0000ff00;
-	else if (mask & 0x00ff0000)
-		bits = 0x00ff0000;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
+
+	if (chan < 8)
+		mask = 0x000000ff;
+	else if (chan < 16)
+		mask = 0x0000ff00;
+	else if (chan < 24)
+		mask = 0x00ff0000;
 	else
-		bits = 0xff000000;
+		mask = 0xff000000;
 
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
-	}
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	ii20k_dio_config(dev, s);
 
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 1eda40a9332e..8f4afadab76a 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -1358,98 +1358,57 @@ static int me4000_dio_insn_bits(struct comedi_device *dev,
 
 static int me4000_dio_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-	unsigned long tmp;
-	int chan = CR_CHAN(insn->chanspec);
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	unsigned int tmp;
+	int ret;
 
-	switch (data[0]) {
-	default:
-		return -EINVAL;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	case INSN_CONFIG_DIO_INPUT:
-	case INSN_CONFIG_DIO_OUTPUT:
-		break;
-	}
+	if (chan < 8)
+		mask = 0x000000ff;
+	else if (chan < 16)
+		mask = 0x0000ff00;
+	else if (chan < 24)
+		mask = 0x00ff0000;
+	else
+		mask = 0xff000000;
 
-	/*
-	 * The input or output configuration of each digital line is
-	 * configured by a special insn_config instruction.  chanspec
-	 * contains the channel to be changed, and data[0] contains the
-	 * value INSN_CONFIG_DIO_INPUT or INSN_CONFIG_DIO_OUTPUT.
-	 * On the ME-4000 it is only possible to switch port wise (8 bit)
-	 */
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	tmp = inl(dev->iobase + ME4000_DIO_CTRL_REG);
+	tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 | ME4000_DIO_CTRL_BIT_MODE_1 |
+		 ME4000_DIO_CTRL_BIT_MODE_2 | ME4000_DIO_CTRL_BIT_MODE_3 |
+		 ME4000_DIO_CTRL_BIT_MODE_4 | ME4000_DIO_CTRL_BIT_MODE_5 |
+		 ME4000_DIO_CTRL_BIT_MODE_6 | ME4000_DIO_CTRL_BIT_MODE_7);
+	if (s->io_bits & 0x000000ff)
+		tmp |= ME4000_DIO_CTRL_BIT_MODE_0;
+	if (s->io_bits & 0x0000ff00)
+		tmp |= ME4000_DIO_CTRL_BIT_MODE_2;
+	if (s->io_bits & 0x00ff0000)
+		tmp |= ME4000_DIO_CTRL_BIT_MODE_4;
+	if (s->io_bits & 0xff000000)
+		tmp |= ME4000_DIO_CTRL_BIT_MODE_6;
 
-	if (data[0] == INSN_CONFIG_DIO_OUTPUT) {
-		if (chan < 8) {
-			s->io_bits |= 0xFF;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 |
-				 ME4000_DIO_CTRL_BIT_MODE_1);
-			tmp |= ME4000_DIO_CTRL_BIT_MODE_0;
-		} else if (chan < 16) {
-			/*
-			 * Chech for optoisolated ME-4000 version.
-			 * If one the first port is a fixed output
-			 * port and the second is a fixed input port.
-			 */
-			if (!inl(dev->iobase + ME4000_DIO_DIR_REG))
-				return -ENODEV;
-
-			s->io_bits |= 0xFF00;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
-				 ME4000_DIO_CTRL_BIT_MODE_3);
-			tmp |= ME4000_DIO_CTRL_BIT_MODE_2;
-		} else if (chan < 24) {
-			s->io_bits |= 0xFF0000;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_4 |
-				 ME4000_DIO_CTRL_BIT_MODE_5);
-			tmp |= ME4000_DIO_CTRL_BIT_MODE_4;
-		} else if (chan < 32) {
-			s->io_bits |= 0xFF000000;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_6 |
-				 ME4000_DIO_CTRL_BIT_MODE_7);
-			tmp |= ME4000_DIO_CTRL_BIT_MODE_6;
-		} else {
-			return -EINVAL;
-		}
-	} else {
-		if (chan < 8) {
-			/*
-			 * Chech for optoisolated ME-4000 version.
-			 * If one the first port is a fixed output
-			 * port and the second is a fixed input port.
-			 */
-			if (!inl(dev->iobase + ME4000_DIO_DIR_REG))
-				return -ENODEV;
-
-			s->io_bits &= ~0xFF;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 |
-				 ME4000_DIO_CTRL_BIT_MODE_1);
-		} else if (chan < 16) {
-			s->io_bits &= ~0xFF00;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
-				 ME4000_DIO_CTRL_BIT_MODE_3);
-		} else if (chan < 24) {
-			s->io_bits &= ~0xFF0000;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_4 |
-				 ME4000_DIO_CTRL_BIT_MODE_5);
-		} else if (chan < 32) {
-			s->io_bits &= ~0xFF000000;
-			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_6 |
-				 ME4000_DIO_CTRL_BIT_MODE_7);
-		} else {
-			return -EINVAL;
-		}
+	/*
+	 * Check for optoisolated ME-4000 version.
+	 * If one the first port is a fixed output
+	 * port and the second is a fixed input port.
+	 */
+	if (inl(dev->iobase + ME4000_DIO_DIR_REG)) {
+		s->io_bits |= 0x000000ff;
+		s->io_bits &= ~0x0000ff00;
+		tmp |= ME4000_DIO_CTRL_BIT_MODE_0;
+		tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
+			 ME4000_DIO_CTRL_BIT_MODE_3);
 	}
 
 	outl(tmp, dev->iobase + ME4000_DIO_CTRL_REG);
 
-	return 1;
+	return insn->n;
 }
 
 /*=============================================================================
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index e8a743c2f9a1..a6f6d4a46587 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -186,38 +186,30 @@ static int me_dio_insn_config(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned int *data)
 {
-	struct me_private_data *dev_private = dev->private;
-	unsigned int mask = 1 << CR_CHAN(insn->chanspec);
-	unsigned int bits;
-	unsigned int port;
+	struct me_private_data *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
 
-	if (mask & 0x0000ffff) {
-		bits = 0x0000ffff;
-		port = ENABLE_PORT_A;
-	} else {
-		bits = 0xffff0000;
-		port = ENABLE_PORT_B;
-	}
+	if (chan < 16)
+		mask = 0x0000ffff;
+	else
+		mask = 0xffff0000;
 
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		dev_private->control_2 &= ~port;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		dev_private->control_2 |= port;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-	}
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
-	/* Update the port configuration */
-	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+	if (s->io_bits & 0x0000ffff)
+		devpriv->control_2 |= ENABLE_PORT_A;
+	else
+		devpriv->control_2 &= ~ENABLE_PORT_A;
+	if (s->io_bits & 0xffff0000)
+		devpriv->control_2 |= ENABLE_PORT_B;
+	else
+		devpriv->control_2 &= ~ENABLE_PORT_B;
+
+	writew(devpriv->control_2, devpriv->me_regbase + ME_CONTROL_2);
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 0025496bc643..b9122fd835e1 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -248,42 +248,35 @@ static int atao_dio_insn_bits(struct comedi_device *dev,
 
 static int atao_dio_insn_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
 	struct atao_private *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask, bit;
-
-	/* The input or output configuration of each digital line is
-	 * configured by a special insn_config instruction.  chanspec
-	 * contains the channel to be changed, and data[0] contains the
-	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
-
-	mask = (chan < 4) ? 0x0f : 0xf0;
-	bit = (chan < 4) ? DOUTEN1 : DOUTEN2;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= mask;
-		devpriv->cfg3 |= bit;
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~mask;
-		devpriv->cfg3 &= ~bit;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-		break;
-	}
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
+
+	if (chan < 4)
+		mask = 0x0f;
+	else
+		mask = 0xf0;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
+
+	if (s->io_bits & 0x0f)
+		devpriv->cfg3 |= DOUTEN1;
+	else
+		devpriv->cfg3 &= ~DOUTEN1;
+	if (s->io_bits & 0xf0)
+		devpriv->cfg3 |= DOUTEN2;
+	else
+		devpriv->cfg3 &= ~DOUTEN2;
 
 	outw(devpriv->cfg3, dev->iobase + ATAO_CFG3);
 
-	return 1;
+	return insn->n;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index cca972ebd010..cc1dc7f66e5b 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -184,39 +184,30 @@ static void enable_chan(struct comedi_device *dev, struct comedi_subdevice *s,
 /* overriding the 8255 insn config */
 static int subdev_3724_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int mask;
-	unsigned int bits;
-
-	mask = 1 << CR_CHAN(insn->chanspec);
-	if (mask & 0x0000ff)
-		bits = 0x0000ff;
-	else if (mask & 0x00ff00)
-		bits = 0x00ff00;
-	else if (mask & 0x0f0000)
-		bits = 0x0f0000;
+	int ret;
+
+	if (chan < 8)
+		mask = 0x0000ff;
+	else if (chan < 16)
+		mask = 0x00ff00;
+	else if (chan < 20)
+		mask = 0x0f0000;
 	else
-		bits = 0xf00000;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-	}
+		mask = 0xf00000;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
 
 	do_3724_config(dev, s, insn->chanspec);
 	enable_chan(dev, s, insn->chanspec);
-	return 1;
+
+	return insn->n;
 }
 
 static int pcm3724_attach(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 6670b865256b..d629463b85a2 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -515,32 +515,35 @@ static int s526_dio_insn_bits(struct comedi_device *dev,
 
 static int s526_dio_insn_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	int group, mask;
+	unsigned int mask;
+	int ret;
+
+	if (chan < 4)
+		mask = 0x0f;
+	else
+		mask = 0xf0;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
+
+	/* bit 10/11 set the group 1/2's mode */
+	if (s->io_bits & 0x0f)
+		s->state |= (1 << 10);
+	else
+		s->state &= ~(1 << 10);
+	if (s->io_bits & 0xf0)
+		s->state |= (1 << 11);
+	else
+		s->state &= ~(1 << 11);
 
-	group = chan >> 2;
-	mask = 0xF << (group << 2);
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		/* bit 10/11 set the group 1/2's mode */
-		s->state |= 1 << (group + 10);
-		s->io_bits |= mask;
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		s->state &= ~(1 << (group + 10)); /* 1 is output, 0 is input. */
-		s->io_bits &= ~mask;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & mask) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
-	}
 	outw(s->state, dev->iobase + REG_DIO);
 
-	return 1;
+	return insn->n;
 }
 
 static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)