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>
This commit is contained in:
Ian Abbott 2013-08-23 14:45:08 +01:00 committed by Greg Kroah-Hartman
parent 16d2d3cbb3
commit 0f3ce1a600
3 changed files with 83 additions and 46 deletions

View File

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

View File

@ -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;
/*
* The insn data is a mask in data[0] and the new data
* in data[1], each channel cooresponding to a bit.
*/
for (i = 0; num_done < nchans && i < devpriv->ndevs; ++i) {
struct bonded_device *bdev = devpriv->devs[i];
/*
* Grab the channel mask and data of only the bits corresponding
* to this subdevice.. need to shift them to zero position of
* course.
*/
/* Bits corresponding to this subdev. */
unsigned int subdev_mask = ((1 << bdev->nchans) - 1);
unsigned int write_mask, data_bits;
n_done = 0;
devs = devpriv->devs;
do {
struct bonded_device *bdev = *devs++;
/* Argh, we have >= LSAMPL_BITS chans.. take all bits */
if (bdev->nchans >= LSAMPL_BITS)
subdev_mask = (unsigned int)(-1);
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;
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;
}
/*
* Get num channels to do for bonded device and set
* up mask and data bits for bonded device.
*/
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;
/*
* Set up for following bonded device (if still have
* channels to read/write).
*/
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;
}

View File

@ -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;
/*
* 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];
*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)