staging: comedi: comedi_bond: handle base channel for insn_bits

If a DIO subdevice has more than 32 channels, its 'insn_bits' handler is
supposed to take account of the base channel from
`CR_CHAN(insn->chanspec)`.  (The comedi core will adjust the base
channel to 0 and shift the mask and data to compensate if the subdevice
has less than or equal to 32 channels.)  The "comedi_bond" driver
currently ignores the base channel and assumes it is 0.

Replace `comedi_dio_bitfield()` in the "kcomedilib" module with
`comedi_dio_bitfield2()` that takes account of the base channel, and
rewrite the "comedi_bond" driver's 'insn_bits' handler
(`bonding_dio_insn_bits()`) to take account of the base channel and use
the new function.

No other modules use `comedi_dio_bitfield()` so it is safe to replace it
with `comedi_dio_bitfield2()`.  The name follows that of the equivalent
function in the user-space comedilib.  If the base channel is non-zero
and the subdevice has less than or equal to 32 channels it needs to
adjust things in the same way as the comedi core (same as `parse_insn()`
in "comedi_fops.c") due to most drivers ignoring the base channel.

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

1 files changed, 31 insertions, 6 deletions

diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
index c7e809b35fd2..cd60677a3ed2 100644
--- a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
+++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
@@ -159,28 +159,53 @@ int comedi_dio_config(struct comedi_device *dev, unsigned int subdev,
 }
 EXPORT_SYMBOL_GPL(comedi_dio_config);
 
-int comedi_dio_bitfield(struct comedi_device *dev, unsigned int subdev,
-			unsigned int mask, unsigned int *bits)
+int comedi_dio_bitfield2(struct comedi_device *dev, unsigned int subdev,
+			 unsigned int mask, unsigned int *bits,
+			 unsigned int base_channel)
 {
 	struct comedi_insn insn;
 	unsigned int data[2];
+	unsigned int n_chan;
+	unsigned int shift;
 	int ret;
 
+	if (subdev >= dev->n_subdevices)
+		return -EINVAL;
+
+	base_channel = CR_CHAN(base_channel);
+	n_chan = comedi_get_n_channels(dev, subdev);
+	if (base_channel >= n_chan)
+		return -EINVAL;
+
 	memset(&insn, 0, sizeof(insn));
 	insn.insn = INSN_BITS;
+	insn.chanspec = base_channel;
 	insn.n = 2;
 	insn.subdev = subdev;
 
 	data[0] = mask;
 	data[1] = *bits;
 
-	ret = comedi_do_insn(dev, &insn, data);
-
-	*bits = data[1];
+	/*
+	 * Most drivers ignore the base channel in insn->chanspec.
+	 * Fix this here if the subdevice has <= 32 channels.
+	 */
+	if (n_chan <= 32) {
+		shift = base_channel;
+		if (shift) {
+			insn.chanspec = 0;
+			data[0] <<= shift;
+			data[1] <<= shift;
+		}
+	} else {
+		shift = 0;
+	}
 
+	ret = comedi_do_insn(dev, &insn, data);
+	*bits = data[1] >> shift;
 	return ret;
 }
-EXPORT_SYMBOL_GPL(comedi_dio_bitfield);
+EXPORT_SYMBOL_GPL(comedi_dio_bitfield2);
 
 int comedi_find_subdevice_by_type(struct comedi_device *dev, int type,
 				  unsigned int subd)