staging: comedi: ni_tio: Allocate shadow regs for each counter chip

The "ni_tio" module contains code to allocate, destroy and operate on a
`struct ni_gpct_device`, which represents a number of counters spread
over one or more blocks (or "chips").  `struct ni_gpct_device` includes
an array member `regs` holding shadow copies of register values.
Unfortunately, this is currently shared by each block of counters so
they interfere with each other.  This is a problem for the "ni_660x"
module, which has 8 counters spread over 2 blocks.  The `regs` storage
needs to be two-dimensional, indexed by block (chip) number and register
number.  (It does not need to be three-dimensional because the registers
for individual counters are intermingled within the block.)

Change the `regs` member to an array pointer that can be indexed like a
two-dimensional array to access the shadow storage for each register in
each block.  Allocate the storage in `ni_gpct_device_construct()` and
free it in `ni_gpct_device_destroy()`.  (`ni_gpct_device_construct()`
can determine the number of blocks from the `num_counters` and
`counters_per_chip` parameters.)

Add new member `num_chips` to hold the number of chips.  Use that to
check that `chip_index` value is in range in the same places that
check the register offset is in range.

Remove the `counters_per_chip` member of `struct ni_gpct_device` as it
is not needed anywhere and could be easily derived from the
`num_counters` and `num_chips` members if required.

Thanks to GitHub user "raabej" (real name unknown) for an initial
implementation of this in the out-of-tree fork of the Comedi drivers.

Signed-off-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 0eb388c..048cb35 100644 (file)
--- a/drivers/staging/comedi/drivers/ni_tio.c
+++ b/drivers/staging/comedi/drivers/ni_tio.c
@@ -224,13 +224,16 @@ static void ni_tio_set_bits_transient(struct ni_gpct *counter,
                                      unsigned int transient)
 {
        struct ni_gpct_device *counter_dev = counter->counter_dev;
+       unsigned int chip = counter->chip_index;
        unsigned long flags;
 
-       if (reg < NITIO_NUM_REGS) {
+       if (reg < NITIO_NUM_REGS && chip < counter_dev->num_chips) {
+               unsigned int *regs = counter_dev->regs[chip];
+
                spin_lock_irqsave(&counter_dev->regs_lock, flags);
-               counter_dev->regs[reg] &= ~mask;
-               counter_dev->regs[reg] |= (value & mask);
-               ni_tio_write(counter, counter_dev->regs[reg] | transient, reg);
+               regs[reg] &= ~mask;
+               regs[reg] |= (value & mask);
+               ni_tio_write(counter, regs[reg] | transient, reg);
                mmiowb();
                spin_unlock_irqrestore(&counter_dev->regs_lock, flags);
        }
@@ -267,12 +270,13 @@ unsigned int ni_tio_get_soft_copy(const struct ni_gpct *counter,
                                  enum ni_gpct_register reg)
 {
        struct ni_gpct_device *counter_dev = counter->counter_dev;
+       unsigned int chip = counter->chip_index;
        unsigned int value = 0;
        unsigned long flags;
 
-       if (reg < NITIO_NUM_REGS) {
+       if (reg < NITIO_NUM_REGS && chip < counter_dev->num_chips) {
                spin_lock_irqsave(&counter_dev->regs_lock, flags);
-               value = counter_dev->regs[reg];
+               value = counter_dev->regs[chip][reg];
                spin_unlock_irqrestore(&counter_dev->regs_lock, flags);
        }
        return value;
@@ -302,6 +306,7 @@ static int ni_m_series_clock_src_select(const struct ni_gpct *counter,
 {
        struct ni_gpct_device *counter_dev = counter->counter_dev;
        unsigned int cidx = counter->counter_index;
+       unsigned int chip = counter->chip_index;
        unsigned int second_gate_reg = NITIO_GATE2_REG(cidx);
        unsigned int clock_source = 0;
        unsigned int src;
@@ -318,7 +323,7 @@ static int ni_m_series_clock_src_select(const struct ni_gpct *counter,
                clock_source = NI_GPCT_TIMEBASE_2_CLOCK_SRC_BITS;
                break;
        case NI_M_TIMEBASE_3_CLK:
-               if (counter_dev->regs[second_gate_reg] & GI_SRC_SUBSEL)
+               if (counter_dev->regs[chip][second_gate_reg] & GI_SRC_SUBSEL)
                        clock_source =
                            NI_GPCT_ANALOG_TRIGGER_OUT_CLOCK_SRC_BITS;
                else
@@ -328,7 +333,7 @@ static int ni_m_series_clock_src_select(const struct ni_gpct *counter,
                clock_source = NI_GPCT_LOGIC_LOW_CLOCK_SRC_BITS;
                break;
        case NI_M_NEXT_GATE_CLK:
-               if (counter_dev->regs[second_gate_reg] & GI_SRC_SUBSEL)
+               if (counter_dev->regs[chip][second_gate_reg] & GI_SRC_SUBSEL)
                        clock_source = NI_GPCT_PXI_STAR_TRIGGER_CLOCK_SRC_BITS;
                else
                        clock_source = NI_GPCT_NEXT_GATE_CLOCK_SRC_BITS;
@@ -721,6 +726,7 @@ static void ni_tio_set_source_subselect(struct ni_gpct *counter,
 {
        struct ni_gpct_device *counter_dev = counter->counter_dev;
        unsigned int cidx = counter->counter_index;
+       unsigned int chip = counter->chip_index;
        unsigned int second_gate_reg = NITIO_GATE2_REG(cidx);
 
        if (counter_dev->variant != ni_gpct_variant_m_series)
@@ -729,18 +735,18 @@ static void ni_tio_set_source_subselect(struct ni_gpct *counter,
                /* Gi_Source_Subselect is zero */
        case NI_GPCT_NEXT_GATE_CLOCK_SRC_BITS:
        case NI_GPCT_TIMEBASE_3_CLOCK_SRC_BITS:
-               counter_dev->regs[second_gate_reg] &= ~GI_SRC_SUBSEL;
+               counter_dev->regs[chip][second_gate_reg] &= ~GI_SRC_SUBSEL;
                break;
                /* Gi_Source_Subselect is one */
        case NI_GPCT_ANALOG_TRIGGER_OUT_CLOCK_SRC_BITS:
        case NI_GPCT_PXI_STAR_TRIGGER_CLOCK_SRC_BITS:
-               counter_dev->regs[second_gate_reg] |= GI_SRC_SUBSEL;
+               counter_dev->regs[chip][second_gate_reg] |= GI_SRC_SUBSEL;
                break;
                /* Gi_Source_Subselect doesn't matter */
        default:
                return;
        }
-       ni_tio_write(counter, counter_dev->regs[second_gate_reg],
+       ni_tio_write(counter, counter_dev->regs[chip][second_gate_reg],
                     second_gate_reg);
 }
 
@@ -1116,6 +1122,7 @@ static int ni_tio_set_other_src(struct ni_gpct *counter, unsigned int index,
 {
        struct ni_gpct_device *counter_dev = counter->counter_dev;
        unsigned int cidx = counter->counter_index;
+       unsigned int chip = counter->chip_index;
        unsigned int abz_reg, shift, mask;
 
        if (counter_dev->variant != ni_gpct_variant_m_series)
@@ -1141,9 +1148,9 @@ static int ni_tio_set_other_src(struct ni_gpct *counter, unsigned int index,
        if (source > 0x1f)
                source = 0x1f;  /* Disable gate */
 
-       counter_dev->regs[abz_reg] &= ~mask;
-       counter_dev->regs[abz_reg] |= (source << shift) & mask;
-       ni_tio_write(counter, counter_dev->regs[abz_reg], abz_reg);
+       counter_dev->regs[chip][abz_reg] &= ~mask;
+       counter_dev->regs[chip][abz_reg] |= (source << shift) & mask;
+       ni_tio_write(counter, counter_dev->regs[chip][abz_reg], abz_reg);
        return 0;
 }
 
@@ -1632,6 +1639,7 @@ int ni_tio_insn_read(struct comedi_device *dev,
        struct ni_gpct_device *counter_dev = counter->counter_dev;
        unsigned int channel = CR_CHAN(insn->chanspec);
        unsigned int cidx = counter->counter_index;
+       unsigned int chip = counter->chip_index;
        int i;
 
        for (i = 0; i < insn->n; i++) {
@@ -1640,10 +1648,12 @@ int ni_tio_insn_read(struct comedi_device *dev,
                        data[i] = ni_tio_read_sw_save_reg(dev, s);
                        break;
                case 1:
-                       data[i] = counter_dev->regs[NITIO_LOADA_REG(cidx)];
+                       data[i] =
+                           counter_dev->regs[chip][NITIO_LOADA_REG(cidx)];
                        break;
                case 2:
-                       data[i] = counter_dev->regs[NITIO_LOADB_REG(cidx)];
+                       data[i] =
+                           counter_dev->regs[chip][NITIO_LOADB_REG(cidx)];
                        break;
                }
        }
@@ -1670,6 +1680,7 @@ int ni_tio_insn_write(struct comedi_device *dev,
        struct ni_gpct_device *counter_dev = counter->counter_dev;
        unsigned int channel = CR_CHAN(insn->chanspec);
        unsigned int cidx = counter->counter_index;
+       unsigned int chip = counter->chip_index;
        unsigned int load_reg;
 
        if (insn->n < 1)
@@ -1690,14 +1701,15 @@ int ni_tio_insn_write(struct comedi_device *dev,
                ni_tio_set_bits_transient(counter, NITIO_CMD_REG(cidx),
                                          0, 0, GI_LOAD);
                /* restore load reg */
-               ni_tio_write(counter, counter_dev->regs[load_reg], load_reg);
+               ni_tio_write(counter, counter_dev->regs[chip][load_reg],
+                            load_reg);
                break;
        case 1:
-               counter_dev->regs[NITIO_LOADA_REG(cidx)] = data[0];
+               counter_dev->regs[chip][NITIO_LOADA_REG(cidx)] = data[0];
                ni_tio_write(counter, data[0], NITIO_LOADA_REG(cidx));
                break;
        case 2:
-               counter_dev->regs[NITIO_LOADB_REG(cidx)] = data[0];
+               counter_dev->regs[chip][NITIO_LOADB_REG(cidx)] = data[0];
                ni_tio_write(counter, data[0], NITIO_LOADB_REG(cidx));
                break;
        default:
@@ -1711,11 +1723,12 @@ void ni_tio_init_counter(struct ni_gpct *counter)
 {
        struct ni_gpct_device *counter_dev = counter->counter_dev;
        unsigned int cidx = counter->counter_index;
+       unsigned int chip = counter->chip_index;
 
        ni_tio_reset_count_and_disarm(counter);
 
        /* initialize counter registers */
-       counter_dev->regs[NITIO_AUTO_INC_REG(cidx)] = 0x0;
+       counter_dev->regs[chip][NITIO_AUTO_INC_REG(cidx)] = 0x0;
        ni_tio_write(counter, 0x0, NITIO_AUTO_INC_REG(cidx));
 
        ni_tio_set_bits(counter, NITIO_CMD_REG(cidx),
@@ -1723,10 +1736,10 @@ void ni_tio_init_counter(struct ni_gpct *counter)
 
        ni_tio_set_bits(counter, NITIO_MODE_REG(cidx), ~0, 0);
 
-       counter_dev->regs[NITIO_LOADA_REG(cidx)] = 0x0;
+       counter_dev->regs[chip][NITIO_LOADA_REG(cidx)] = 0x0;
        ni_tio_write(counter, 0x0, NITIO_LOADA_REG(cidx));
 
-       counter_dev->regs[NITIO_LOADB_REG(cidx)] = 0x0;
+       counter_dev->regs[chip][NITIO_LOADB_REG(cidx)] = 0x0;
        ni_tio_write(counter, 0x0, NITIO_LOADB_REG(cidx));
 
        ni_tio_set_bits(counter, NITIO_INPUT_SEL_REG(cidx), ~0, 0);
@@ -1735,7 +1748,7 @@ void ni_tio_init_counter(struct ni_gpct *counter)
                ni_tio_set_bits(counter, NITIO_CNT_MODE_REG(cidx), ~0, 0);
 
        if (ni_tio_has_gate2_registers(counter_dev)) {
-               counter_dev->regs[NITIO_GATE2_REG(cidx)] = 0x0;
+               counter_dev->regs[chip][NITIO_GATE2_REG(cidx)] = 0x0;
                ni_tio_write(counter, 0x0, NITIO_GATE2_REG(cidx));
        }
 
@@ -1776,9 +1789,16 @@ ni_gpct_device_construct(struct comedi_device *dev,
 
        spin_lock_init(&counter_dev->regs_lock);
 
+       counter_dev->num_counters = num_counters;
+       counter_dev->num_chips = DIV_ROUND_UP(num_counters, counters_per_chip);
+
        counter_dev->counters = kcalloc(num_counters, sizeof(*counter),
                                        GFP_KERNEL);
-       if (!counter_dev->counters) {
+       counter_dev->regs = kcalloc(counter_dev->num_chips,
+                                   sizeof(*counter_dev->regs), GFP_KERNEL);
+       if (!counter_dev->regs || !counter_dev->counters) {
+               kfree(counter_dev->regs);
+               kfree(counter_dev->counters);
                kfree(counter_dev);
                return NULL;
        }
@@ -1790,8 +1810,6 @@ ni_gpct_device_construct(struct comedi_device *dev,
                counter->counter_index = i % counters_per_chip;
                spin_lock_init(&counter->lock);
        }
-       counter_dev->num_counters = num_counters;
-       counter_dev->counters_per_chip = counters_per_chip;
 
        return counter_dev;
 }
@@ -1801,6 +1819,7 @@ void ni_gpct_device_destroy(struct ni_gpct_device *counter_dev)
 {
        if (!counter_dev)
                return;
+       kfree(counter_dev->regs);
        kfree(counter_dev->counters);
        kfree(counter_dev);
 }

diff --git a/drivers/staging/comedi/drivers/ni_tio.h b/drivers/staging/comedi/drivers/ni_tio.h
index 1e85d0a..e7b0571 100644 (file)
--- a/drivers/staging/comedi/drivers/ni_tio.h
+++ b/drivers/staging/comedi/drivers/ni_tio.h
@@ -107,8 +107,8 @@ struct ni_gpct_device {
        enum ni_gpct_variant variant;
        struct ni_gpct *counters;
        unsigned int num_counters;
-       unsigned int counters_per_chip;
-       unsigned int regs[NITIO_NUM_REGS];
+       unsigned int num_chips;
+       unsigned int (*regs)[NITIO_NUM_REGS]; /* [num_chips][NITIO_NUM_REGS] */
        spinlock_t regs_lock;           /* protects 'regs' */
        const struct ni_route_tables *routing_tables; /* link to routes */
 };