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

This reverts commit f21c53945cb95f66faa9636af5f23cb00ba73019.

I applied the wrong patch :(

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

15 files changed, 433 insertions, 263 deletions

diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 2f070fdbbb1d..1fa29ac4eb5d 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -184,29 +184,39 @@ 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;
-	int ret;
+	unsigned int bits;
 
-	if (chan < 8)
-		mask = 0x0000ff;
-	else if (chan < 16)
-		mask = 0x00ff00;
-	else if (chan < 20)
-		mask = 0x0f0000;
+	mask = 1 << CR_CHAN(insn->chanspec);
+	if (mask & 0x0000ff)
+		bits = 0x0000ff;
+	else if (mask & 0x00ff00)
+		bits = 0x00ff00;
+	else if (mask & 0x0f0000)
+		bits = 0x0f0000;
 	else
-		mask = 0xf00000;
-
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+		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;
+	}
 
 	subdev_8255_do_config(dev, s);
 
-	return insn->n;
+	return 1;
 }
 
 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 96523744b8de..43296a6e7d4e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -60,22 +60,36 @@ static int apci16xx_insn_config(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	int ret;
+	unsigned int chan_mask = 1 << CR_CHAN(insn->chanspec);
+	unsigned int bits;
 
-	if (chan < 8)
-		mask = 0x000000ff;
-	else if (chan < 16)
-		mask = 0x0000ff00;
-	else if (chan < 24)
-		mask = 0x00ff0000;
+	/*
+	 * 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;
 	else
-		mask = 0xff000000;
-
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+		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;
+	}
 
 	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 cf5dd10eaf91..dbc0678027ca 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -686,28 +686,38 @@ static int apci3xxx_dio_insn_config(struct comedi_device *dev,
 				    unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	int ret;
+	unsigned int mask = 1 << chan;
+	unsigned int bits;
 
 	/*
 	 * 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 (chan < 16) {
-		if (data[0] != INSN_CONFIG_DIO_QUERY)
-			return -EINVAL;
-	} else {
-		/* changing any channel in port 2 changes the entire port */
-		mask = 0xff0000;
+	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;
 	}
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
-
 	/* update port 2 configuration */
-	outl((s->io_bits >> 24) & 0xff, dev->iobase + 224);
+	if (bits)
+		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 b793d6987b84..f914fb1f571b 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -180,29 +180,38 @@ 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 short mode;
-	int ret;
+	unsigned int bits;
+	unsigned short dio_mode;
 
-	if (chan < 8)
-		mask = 0x00ff;
+	mask = 1 << CR_CHAN(insn->chanspec);
+	if (mask & 0x00FF)
+		bits = 0x00FF;
 	else
-		mask = 0xff00;
+		bits = 0xFF00;
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+	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;
+	}
 
 	/* update hardware DIO mode */
-	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;
+	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;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index c1f723e86146..32c490b27b55 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -976,26 +976,34 @@ 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;
-	int ret;
-
-	if (chan < 8)
-		mask = 0x0000ff;
-	else if (chan < 16)
-		mask = 0x00ff00;
-	else if (chan < 20)
-		mask = 0x0f0000;
+	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;
 	else
-		mask = 0xf00000;
-
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
-
+		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;
+	}
 	dio200_subdev_8255_set_dir(dev, s);
-
-	return insn->n;
+	return 1;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 0ce93da70847..05e01a3b5af6 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -341,22 +341,33 @@ 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;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	int ret;
+	int chan = CR_CHAN(insn->chanspec);
+	int bits;
 
 	if (chan < 4)
-		mask = 0x0f;
+		bits = 0x0f;
 	else
-		mask = 0xf0;
+		bits = 0xf0;
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+	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;
+	}
 
 	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 38918a1198aa..e0e7beab7274 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -551,19 +551,32 @@ 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 ret;
+	int which = 0;
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, 0xff);
-	if (ret)
-		return ret;
+	if (s == &dev->subdevices[3])
+		which = 1;
 
-	dt2801_writecmd(dev, s->io_bits ? DT_C_SET_DIGOUT : DT_C_SET_DIGIN);
-	dt2801_writedata(dev, (s == &dev->subdevices[3]) ? 1 : 0);
+	/* 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);
 
-	return insn->n;
+	return 1;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 64ef87598b60..e4748da1993b 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -642,23 +642,32 @@ 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)
 {
-	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));
+	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);
 
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 559bf5583530..cdcc8f42e209 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -224,26 +224,37 @@ 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)
 {
-	int ret;
+	struct hpdi_private *devpriv = dev->private;
 
 	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 insn->n;
+	return -EINVAL;
 }
 
 static void disable_plx_interrupts(struct comedi_device *dev)
@@ -662,7 +673,9 @@ 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)
 {
-	if (s->io_bits)
+	struct hpdi_private *devpriv = dev->private;
+
+	if (devpriv->dio_config_output)
 		return -EINVAL;
 	else
 		return di_cmd_test(dev, s, cmd);
@@ -733,7 +746,9 @@ static int di_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static int hpdi_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	if (s->io_bits)
+	struct hpdi_private *devpriv = dev->private;
+
+	if (devpriv->dio_config_output)
 		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 5c3a318b4640..954c5397f33c 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -350,22 +350,31 @@ static int ii20k_dio_insn_config(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	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;
+	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;
 	else
-		mask = 0xff000000;
+		bits = 0xff000000;
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+	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;
+	}
 
 	ii20k_dio_config(dev, s);
 
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 8f4afadab76a..1eda40a9332e 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -1358,57 +1358,98 @@ 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 int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	unsigned int tmp;
-	int ret;
+	unsigned long tmp;
+	int chan = CR_CHAN(insn->chanspec);
 
-	if (chan < 8)
-		mask = 0x000000ff;
-	else if (chan < 16)
-		mask = 0x0000ff00;
-	else if (chan < 24)
-		mask = 0x00ff0000;
-	else
-		mask = 0xff000000;
+	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;
+	}
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+	/*
+	 * 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)
+	 */
 
 	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;
 
-	/*
-	 * 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);
+	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;
+		}
 	}
 
 	outl(tmp, dev->iobase + ME4000_DIO_CTRL_REG);
 
-	return insn->n;
+	return 1;
 }
 
 /*=============================================================================
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index a6f6d4a46587..e8a743c2f9a1 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -186,30 +186,38 @@ static int me_dio_insn_config(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned int *data)
 {
-	struct me_private_data *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	int ret;
-
-	if (chan < 16)
-		mask = 0x0000ffff;
-	else
-		mask = 0xffff0000;
+	struct me_private_data *dev_private = dev->private;
+	unsigned int mask = 1 << CR_CHAN(insn->chanspec);
+	unsigned int bits;
+	unsigned int port;
 
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+	if (mask & 0x0000ffff) {
+		bits = 0x0000ffff;
+		port = ENABLE_PORT_A;
+	} else {
+		bits = 0xffff0000;
+		port = ENABLE_PORT_B;
+	}
 
-	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;
+	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;
+	}
 
-	writew(devpriv->control_2, devpriv->me_regbase + ME_CONTROL_2);
+	/* Update the port configuration */
+	writew(dev_private->control_2, dev_private->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 b9122fd835e1..0025496bc643 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -248,35 +248,42 @@ 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;
-	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;
+	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;
+	}
 
 	outw(devpriv->cfg3, dev->iobase + ATAO_CFG3);
 
-	return insn->n;
+	return 1;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index cc1dc7f66e5b..cca972ebd010 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -184,30 +184,39 @@ 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;
-	int ret;
-
-	if (chan < 8)
-		mask = 0x0000ff;
-	else if (chan < 16)
-		mask = 0x00ff00;
-	else if (chan < 20)
-		mask = 0x0f0000;
+	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;
 	else
-		mask = 0xf00000;
-
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
+		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;
+	}
 
 	do_3724_config(dev, s, insn->chanspec);
 	enable_chan(dev, s, insn->chanspec);
-
-	return insn->n;
+	return 1;
 }
 
 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 d629463b85a2..6670b865256b 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -515,35 +515,32 @@ 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);
-	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);
+	int group, mask;
 
+	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 insn->n;
+	return 1;
 }
 
 static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)