From 0f3ce1a600fc1aca996a550f332c9f0c712ea80a Mon Sep 17 00:00:00 2001 From: Ian Abbott Date: Fri, 23 Aug 2013 14:45:08 +0100 Subject: [PATCH] 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 Signed-off-by: Greg Kroah-Hartman --- drivers/staging/comedi/comedilib.h | 5 +- drivers/staging/comedi/drivers/comedi_bond.c | 87 +++++++++++-------- .../comedi/kcomedilib/kcomedilib_main.c | 37 ++++++-- 3 files changed, 83 insertions(+), 46 deletions(-) diff --git a/drivers/staging/comedi/comedilib.h b/drivers/staging/comedi/comedilib.h index 47b164024182..56baf852ecf5 100644 --- a/drivers/staging/comedi/comedilib.h +++ b/drivers/staging/comedi/comedilib.h @@ -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); diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c index a1c51a2e9c59..8e2696c85720 100644 --- a/drivers/staging/comedi/drivers/comedi_bond.c +++ b/drivers/staging/comedi/drivers/comedi_bond.c @@ -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; - - /* 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; + n_done = 0; + devs = devpriv->devs; + do { + struct bonded_device *bdev = *devs++; - /* 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]; + 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; - 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; } 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) -- 2.30.2