Reworked INSN_BITS to support dio subdevices with > 32 channels

by specifying a base channel in the insn.chanspec.

diff --git a/comedi/drivers.c b/comedi/drivers.c
index ae82fc8709af4df83cc0081aaacb00dff3f31572..87ccca7433e6a32c63d9a5ed5833a50d23523004 100644 (file)
--- 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 ef01f9be29e28db38fa383ab92403cd5592560ab..c62c4f7168f42127cddcdc907a08b37a874d5514 100644 (file)
--- 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;
 }