Commit 0f3ce1a6 authored by Ian Abbott's avatar Ian Abbott Committed by Greg Kroah-Hartman
Browse files

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: default avatarIan Abbott <abbotti@mev.co.uk>
Signed-off-by: default avatarGreg Kroah-Hartman <gregkh@linuxfoundation.org>
parent 16d2d3cb

drivers/staging/comedi/comedilib.h

+3 −2
Original line number Diff line number Diff line
@@ -25,8 +25,9 @@ int comedi_dio_get_config(struct comedi_device *dev, unsigned int subdev, 
			  unsigned int chan, unsigned int *io);

int comedi_dio_config(struct comedi_device *dev, unsigned int subdev,

		      unsigned int chan, unsigned int io);

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);

int comedi_find_subdevice_by_type(struct comedi_device *dev, int type,

				  unsigned int subd);

int comedi_get_n_channels(struct comedi_device *dev, unsigned int subdevice);

drivers/staging/comedi/drivers/comedi_bond.c

+49 −38
Original line number Diff line number Diff line
@@ -73,48 +73,59 @@ static int bonding_dio_insn_bits(struct comedi_device *dev, 
				 struct comedi_insn *insn, unsigned int *data)

{

	struct comedi_bond_private *devpriv = dev->private;

#define LSAMPL_BITS (sizeof(unsigned int)*8)

	unsigned nchans = LSAMPL_BITS, num_done = 0, i;

	unsigned int n_left, n_done, base_chan;

	unsigned int write_mask, data_bits;

	struct bonded_device **devs;



	if (devpriv->nchans < nchans)

		nchans = devpriv->nchans;

	write_mask = data[0];

	data_bits = data[1];

	base_chan = CR_CHAN(insn->chanspec);

	/* do a maximum of 32 channels, starting from base_chan. */

	n_left = devpriv->nchans - base_chan;

	if (n_left > 32)

		n_left = 32;



	n_done = 0;

	devs = devpriv->devs;

	do {

		struct bonded_device *bdev = *devs++;



		if (base_chan < bdev->nchans) {

			/* base channel falls within bonded device */

			unsigned int b_chans, b_mask, b_write_mask, b_data_bits;

			int ret;



			/*

	 * The insn data is a mask in data[0] and the new data

	 * in data[1], each channel cooresponding to a bit.

			 * Get num channels to do for bonded device and set

			 * up mask and data bits for bonded device.

			 */

	for (i = 0; num_done < nchans && i < devpriv->ndevs; ++i) {

		struct bonded_device *bdev = devpriv->devs[i];

			b_chans = bdev->nchans - base_chan;

			if (b_chans > n_left)

				b_chans = n_left;

			b_mask = (1U << b_chans) - 1;

			b_write_mask = (write_mask >> n_done) & b_mask;

			b_data_bits = (data_bits >> n_done) & b_mask;

			/* Read/Write the new digital lines. */

			ret = comedi_dio_bitfield2(bdev->dev, bdev->subdev,

						   b_write_mask, &b_data_bits,

						   base_chan);

			if (ret < 0)

				return ret;

			/* Place read bits into data[1]. */

			data[1] &= ~(b_mask << n_done);

			data[1] |= (b_data_bits & b_mask) << n_done;

			/*

		 * Grab the channel mask and data of only the bits corresponding

		 * to this subdevice.. need to shift them to zero position of

		 * course.

			 * Set up for following bonded device (if still have

			 * channels to read/write).

			 */

		/* Bits corresponding to this subdev. */

		unsigned int subdev_mask = ((1 << bdev->nchans) - 1);

		unsigned int write_mask, data_bits;



		/* Argh, we have >= LSAMPL_BITS chans.. take all bits */

		if (bdev->nchans >= LSAMPL_BITS)

			subdev_mask = (unsigned int)(-1);



		write_mask = (data[0] >> num_done) & subdev_mask;

		data_bits = (data[1] >> num_done) & subdev_mask;



		/* Read/Write the new digital lines */

		if (comedi_dio_bitfield(bdev->dev, bdev->subdev, write_mask,

					&data_bits) != 2)

			return -EINVAL;



		/* Make room for the new bits in data[1], the return value */

		data[1] &= ~(subdev_mask << num_done);

		/* Put the bits in the return value */

		data[1] |= (data_bits & subdev_mask) << num_done;

		/* Save the new bits to the saved state.. */

		s->state = data[1];



		num_done += bdev->nchans;

			base_chan = 0;

			n_done += b_chans;

			n_left -= b_chans;

		} else {

			/* Skip bonded devices before base channel. */

			base_chan -= bdev->nchans;

		}

	} while (n_left);



	return insn->n;

}

drivers/staging/comedi/kcomedilib/kcomedilib_main.c

+31 −6
Original line number Diff line number Diff line
@@ -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)

CRA Git | Maintained and supported by SUSTech CRA and CCSE