staging: comedi: ni_tio: fix ni_tio_insn_read()

The comedi core expects the (*insn_read) operations to read insn->n
values and return the number of values read.

Fix this function to work line the core expects.

For aesthetics, factor out the code that reads the SM_Save_Reg.

Signed-off-by: H Hartley Sweeten <hsweeten@visionengravers.com>
Reviewed-by: Ian Abbott <abbotti@mev.co.uk>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/staging/comedi/drivers/ni_tio.c b/drivers/staging/comedi/drivers/ni_tio.c
index 694bd5142cc5a52f57bb7d5552c19c7559f456eb..da6602392fb3ec8c9a9db9d8e53bb30dfb4752d5 100644 (file)
--- a/drivers/staging/comedi/drivers/ni_tio.c
+++ b/drivers/staging/comedi/drivers/ni_tio.c
@@ -1512,6 +1512,38 @@ int ni_tio_insn_config(struct comedi_device *dev,
 }
 EXPORT_SYMBOL_GPL(ni_tio_insn_config);
 
+static unsigned int ni_tio_read_sw_save_reg(struct comedi_device *dev,
+                                           struct comedi_subdevice *s)
+{
+       struct ni_gpct *counter = s->private;
+       unsigned cidx = counter->counter_index;
+       unsigned int first_read;
+       unsigned int second_read;
+       unsigned int correct_read;
+
+       ni_tio_set_bits(counter, NITIO_CMD_REG(cidx), Gi_Save_Trace_Bit, 0);
+       ni_tio_set_bits(counter, NITIO_CMD_REG(cidx),
+                       Gi_Save_Trace_Bit, Gi_Save_Trace_Bit);
+
+       /*
+        * The count doesn't get latched until the next clock edge, so it is
+        * possible the count may change (once) while we are reading. Since
+        * the read of the SW_Save_Reg isn't atomic (apparently even when it's
+        * a 32 bit register according to 660x docs), we need to read twice
+        * and make sure the reading hasn't changed. If it has, a third read
+        * will be correct since the count value will definitely have latched
+        * by then.
+        */
+       first_read = read_register(counter, NITIO_SW_SAVE_REG(cidx));
+       second_read = read_register(counter, NITIO_SW_SAVE_REG(cidx));
+       if (first_read != second_read)
+               correct_read = read_register(counter, NITIO_SW_SAVE_REG(cidx));
+       else
+               correct_read = first_read;
+
+       return correct_read;
+}
+
 int ni_tio_insn_read(struct comedi_device *dev,
                     struct comedi_subdevice *s,
                     struct comedi_insn *insn,
@@ -1519,42 +1551,24 @@ int ni_tio_insn_read(struct comedi_device *dev,
 {
        struct ni_gpct *counter = s->private;
        struct ni_gpct_device *counter_dev = counter->counter_dev;
-       const unsigned channel = CR_CHAN(insn->chanspec);
+       unsigned int channel = CR_CHAN(insn->chanspec);
        unsigned cidx = counter->counter_index;
-       unsigned first_read;
-       unsigned second_read;
-       unsigned correct_read;
+       int i;
 
-       if (insn->n < 1)
-               return 0;
-       switch (channel) {
-       case 0:
-               ni_tio_set_bits(counter, NITIO_CMD_REG(cidx),
-                               Gi_Save_Trace_Bit, 0);
-               ni_tio_set_bits(counter, NITIO_CMD_REG(cidx),
-                               Gi_Save_Trace_Bit, Gi_Save_Trace_Bit);
-               /* The count doesn't get latched until the next clock edge, so it is possible the count
-                  may change (once) while we are reading.  Since the read of the SW_Save_Reg isn't
-                  atomic (apparently even when it's a 32 bit register according to 660x docs),
-                  we need to read twice and make sure the reading hasn't changed.  If it has,
-                  a third read will be correct since the count value will definitely have latched by then. */
-               first_read = read_register(counter, NITIO_SW_SAVE_REG(cidx));
-               second_read = read_register(counter, NITIO_SW_SAVE_REG(cidx));
-               if (first_read != second_read)
-                       correct_read =
-                           read_register(counter, NITIO_SW_SAVE_REG(cidx));
-               else
-                       correct_read = first_read;
-               data[0] = correct_read;
-               return 0;
-       case 1:
-               data[0] = counter_dev->regs[NITIO_LOADA_REG(cidx)];
-               break;
-       case 2:
-               data[0] = counter_dev->regs[NITIO_LOADB_REG(cidx)];
-               break;
+       for (i = 0; i < insn->n; i++) {
+               switch (channel) {
+               case 0:
+                       data[i] = ni_tio_read_sw_save_reg(dev, s);
+                       break;
+               case 1:
+                       data[i] = counter_dev->regs[NITIO_LOADA_REG(cidx)];
+                       break;
+               case 2:
+                       data[i] = counter_dev->regs[NITIO_LOADB_REG(cidx)];
+                       break;
+               }
        }
-       return 0;
+       return insn->n;
 }
 EXPORT_SYMBOL_GPL(ni_tio_insn_read);