diff options
Diffstat (limited to 'drivers/staging/comedi/kcomedilib/kcomedilib_main.c')
-rw-r--r-- | drivers/staging/comedi/kcomedilib/kcomedilib_main.c | 60 |
1 files changed, 52 insertions, 8 deletions
diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c index da8988c6bf50..cd60677a3ed2 100644 --- a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c +++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c @@ -22,8 +22,6 @@ #include <linux/kernel.h> #include <linux/sched.h> #include <linux/fcntl.h> -#include <linux/delay.h> -#include <linux/ioport.h> #include <linux/mm.h> #include <linux/io.h> @@ -125,6 +123,27 @@ error: return ret; } +int comedi_dio_get_config(struct comedi_device *dev, unsigned int subdev, + unsigned int chan, unsigned int *io) +{ + struct comedi_insn insn; + unsigned int data[2]; + int ret; + + memset(&insn, 0, sizeof(insn)); + insn.insn = INSN_CONFIG; + insn.n = 2; + insn.subdev = subdev; + insn.chanspec = CR_PACK(chan, 0, 0); + data[0] = INSN_CONFIG_DIO_QUERY; + data[1] = 0; + ret = comedi_do_insn(dev, &insn, data); + if (ret >= 0) + *io = data[1]; + return ret; +} +EXPORT_SYMBOL_GPL(comedi_dio_get_config); + int comedi_dio_config(struct comedi_device *dev, unsigned int subdev, unsigned int chan, unsigned int io) { @@ -140,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) |