staging: comedi: Make INSN_BITS behavior consistent across drivers

Most comedi hardware drivers that support the INSN_BITS instruction
ignore the base channel (specified by insn->chanspec) and assume it is
0.  The base channel is supposed to affect how the mask (in data[0]) and
bits (in data[1]) are treated.  Bit 0 applies to the base channel, bit 1
applies to base channel plus 1, etc.

For subdevices with no more than 32 channels, this patch modifies the
chanspec and data before presenting it to the hardware driver, and
modifies the data bits read back by the hardware driver (into data[1]).
This makes it appear to the hardware driver that the base channel was
set to 0.

For subdevices with more than 32 channels, the instruction is left
unmodified, as it is assumed that the hardware driver takes note of the
base channel in this case in order to provide access beyond channel 31.

Signed-off-by: Ian Abbott <abbotti@mev.co.uk>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

1 files changed, 21 insertions, 2 deletions

diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 093032ba521a..a4ceb29c358e 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -910,9 +910,28 @@ static int parse_insn(struct comedi_device *dev, struct comedi_insn *insn,
 		case INSN_BITS:
 			if (insn->n != 2) {
 				ret = -EINVAL;
-				break;
+			} else {
+				/* Most drivers ignore the base channel in
+				 * insn->chanspec.  Fix this here if
+				 * the subdevice has <= 32 channels.  */
+				unsigned int shift;
+				unsigned int orig_mask;
+
+				orig_mask = data[0];
+				if (s->n_chan <= 32) {
+					shift = CR_CHAN(insn->chanspec);
+					if (shift > 0) {
+						insn->chanspec = 0;
+						data[0] <<= shift;
+						data[1] <<= shift;
+					}
+				} else
+					shift = 0;
+				ret = s->insn_bits(dev, s, insn, data);
+				data[0] = orig_mask;
+				if (shift > 0)
+					data[1] >>= shift;
 			}
-			ret = s->insn_bits(dev, s, insn, data);
 			break;
 		case INSN_CONFIG:
 			ret = check_insn_config_length(insn, data);