From 00ced38d9ec0479e1712ed0edc099db863693519 Mon Sep 17 00:00:00 2001 From: Frank Mori Hess Date: Tue, 17 Oct 2006 17:21:18 +0000 Subject: [PATCH] Reworked INSN_BITS to support dio subdevices with > 32 channels by specifying a base channel in the insn.chanspec. --- comedi/drivers.c | 19 ++++++------ comedi/drivers/ni_65xx.c | 67 ++++++++++++++++++++++++++-------------- 2 files changed, 52 insertions(+), 34 deletions(-) diff --git a/comedi/drivers.c b/comedi/drivers.c index ae82fc87..87ccca74 100644 --- a/comedi/drivers.c +++ b/comedi/drivers.c @@ -354,17 +354,16 @@ static int insn_rw_emulate_bits(comedi_device *dev,comedi_subdevice *s, { comedi_insn new_insn; int ret; - unsigned int chan; static const unsigned channels_per_bitfield = 32; - chan = CR_CHAN(insn->chanspec); - const unsigned num_bitfields = (chan + channels_per_bitfield) / channels_per_bitfield; - const unsigned array_offset = 2 * (num_bitfields - 1); - lsampl_t new_data[2 * num_bitfields]; + unsigned chan = CR_CHAN(insn->chanspec); + const unsigned base_bitfield_channel = (chan < channels_per_bitfield) ? 0 : chan; + lsampl_t new_data[2]; memset(new_data, 0, sizeof(new_data)); memset(&new_insn, 0, sizeof(new_insn)); new_insn.insn = INSN_BITS; - new_insn.n = 2 * num_bitfields; + new_insn.chanspec = base_bitfield_channel; + new_insn.n = 2; new_insn.data = new_data; new_insn.subdev = insn->subdev; @@ -372,15 +371,15 @@ static int insn_rw_emulate_bits(comedi_device *dev,comedi_subdevice *s, { if(!(s->subdev_flags & SDF_WRITABLE)) return -EINVAL; - new_data[array_offset] = 1 << (chan % channels_per_bitfield); /* mask */ - new_data[array_offset + 1] = data[0] ? (1 << (chan % channels_per_bitfield)) : 0; /* bits */ + new_data[0] = 1 << (chan - base_bitfield_channel); /* mask */ + new_data[1] = data[0] ? (1 << (chan - base_bitfield_channel)) : 0; /* bits */ } ret = s->insn_bits(dev,s,&new_insn,new_data); - if(ret<0)return ret; + if(ret < 0) return ret; if(insn->insn == INSN_READ){ - data[0] = (new_data[array_offset + 1] >> (chan % channels_per_bitfield)) & 1; + data[0] = (new_data[1] >> (chan - base_bitfield_channel)) & 1; } return 1; diff --git a/comedi/drivers/ni_65xx.c b/comedi/drivers/ni_65xx.c index ef01f9be..c62c4f71 100644 --- a/comedi/drivers/ni_65xx.c +++ b/comedi/drivers/ni_65xx.c @@ -363,33 +363,52 @@ static int ni_65xx_dio_insn_config(comedi_device *dev,comedi_subdevice *s, static int ni_65xx_dio_insn_bits(comedi_device *dev,comedi_subdevice *s, comedi_insn *insn,lsampl_t *data) { - const unsigned elements_per_bitfield = 2; - if(insn->n % elements_per_bitfield) return -EINVAL; - const unsigned num_bitfields = insn->n / elements_per_bitfield; - unsigned i; - for(i = 0; i < num_bitfields; ++i) - { - const unsigned ports_per_bitfield = 4; - const unsigned array_offset = i * elements_per_bitfield; - unsigned read_bits = 0; - unsigned j; - for(j = 0; j < ports_per_bitfield; ++j) + if(insn->n != 2) return -EINVAL; + const unsigned base_bitfield_channel = CR_CHAN(insn->chanspec); + const unsigned max_ports_per_bitfield = 5; + unsigned read_bits = 0; + unsigned j; + for(j = 0; j < max_ports_per_bitfield; ++j) + { + const unsigned port = sprivate(s)->base_port + ni_65xx_port_by_channel(base_bitfield_channel) + j; + if(port >= ni_65xx_total_num_ports(board(dev))) break; + const unsigned base_port_channel = port * ni_65xx_channels_per_port; + unsigned port_mask = data[0]; + unsigned port_data = data[1]; + int bitshift = base_port_channel - base_bitfield_channel; + if(bitshift >= 32 || bitshift <= -32) break; + if(bitshift > 0) { - const unsigned port = sprivate(s)->base_port + i * ports_per_bitfield + j; - if(port >= ni_65xx_total_num_ports(board(dev))) break; - const unsigned port_mask = (data[array_offset] >> (j * 8)) & 0xff; - if(port_mask) - { - private(dev)->output_bits[port] &= ~port_mask; - private(dev)->output_bits[port] |= (data[array_offset + 1] >> (j * 8)) & port_mask; - unsigned bits = private(dev)->output_bits[port]; - if(board(dev)->invert_outputs) bits = ~bits; - writeb(bits, private(dev)->mite->daq_io_addr + Port_Data(port)); - } - read_bits |= readb(private(dev)->mite->daq_io_addr + Port_Data(port)) << (j * 8); + port_mask >>= bitshift; + port_data >>= bitshift; + }else + { + port_mask <<= -bitshift; + port_data <<= -bitshift; + } + port_mask &= 0xff; + port_data &= 0xff; + if(port_mask) + { + private(dev)->output_bits[port] &= ~port_mask; + private(dev)->output_bits[port] |= port_data & port_mask; + unsigned bits = private(dev)->output_bits[port]; + if(board(dev)->invert_outputs) bits = ~bits; + writeb(bits, private(dev)->mite->daq_io_addr + Port_Data(port)); +// rt_printk("wrote 0x%x to port %i\n", bits, port); + } + unsigned port_read_bits = readb(private(dev)->mite->daq_io_addr + Port_Data(port)); +// rt_printk("read 0x%x from port %i\n", port_read_bits, port); + if(bitshift > 0) + { + port_read_bits <<= bitshift; + }else + { + port_read_bits >>= -bitshift; } - data[array_offset + 1] = read_bits; + read_bits |= port_read_bits; } + data[1] = read_bits; return insn->n; } -- 2.26.2