diff options
Diffstat (limited to 'drivers/counter')
| -rw-r--r-- | drivers/counter/104-quad-8.c | 97 |
1 files changed, 45 insertions, 52 deletions
diff --git a/drivers/counter/104-quad-8.c b/drivers/counter/104-quad-8.c index 0188c9c4e4cb..d6ede651cc5e 100644 --- a/drivers/counter/104-quad-8.c +++ b/drivers/counter/104-quad-8.c @@ -21,6 +21,8 @@ #include <linux/types.h> #include <linux/spinlock.h> +#include <asm/unaligned.h> + #define QUAD8_EXTENT 32 static unsigned int base[max_num_isa_dev(QUAD8_EXTENT)]; @@ -232,52 +234,60 @@ static int quad8_count_read(struct counter_device *counter, struct quad8 *const priv = counter_priv(counter); struct channel_reg __iomem *const chan = priv->reg->channel + count->id; unsigned long irqflags; - int i; - - *val = 0; + u8 value[3]; spin_lock_irqsave(&priv->lock, irqflags); iowrite8(SELECT_RLD | RESET_BP | TRANSFER_CNTR_TO_OL, &chan->control); - - for (i = 0; i < 3; i++) - *val |= (unsigned long)ioread8(&chan->data) << (8 * i); + ioread8_rep(&chan->data, value, sizeof(value)); spin_unlock_irqrestore(&priv->lock, irqflags); + *val = get_unaligned_le24(value); + return 0; } +static void quad8_preset_register_set(struct quad8 *const priv, const size_t id, + const unsigned long preset) +{ + struct channel_reg __iomem *const chan = priv->reg->channel + id; + u8 value[3]; + + put_unaligned_le24(preset, value); + + iowrite8(SELECT_RLD | RESET_BP, &chan->control); + iowrite8_rep(&chan->data, value, sizeof(value)); +} + +static void quad8_flag_register_reset(struct quad8 *const priv, const size_t id) +{ + struct channel_reg __iomem *const chan = priv->reg->channel + id; + + iowrite8(SELECT_RLD | RESET_BT_CT_CPT_S_IDX, &chan->control); + iowrite8(SELECT_RLD | RESET_E, &chan->control); +} + static int quad8_count_write(struct counter_device *counter, struct counter_count *count, u64 val) { struct quad8 *const priv = counter_priv(counter); struct channel_reg __iomem *const chan = priv->reg->channel + count->id; unsigned long irqflags; - int i; if (val > LS7267_CNTR_MAX) return -ERANGE; spin_lock_irqsave(&priv->lock, irqflags); - iowrite8(SELECT_RLD | RESET_BP, &chan->control); - /* Counter can only be set via Preset Register */ - for (i = 0; i < 3; i++) - iowrite8(val >> (8 * i), &chan->data); - + quad8_preset_register_set(priv, count->id, val); iowrite8(SELECT_RLD | TRANSFER_PR_TO_CNTR, &chan->control); - iowrite8(SELECT_RLD | RESET_BP, &chan->control); + quad8_flag_register_reset(priv, count->id); /* Set Preset Register back to original value */ - val = priv->preset[count->id]; - for (i = 0; i < 3; i++) - iowrite8(val >> (8 * i), &chan->data); - - iowrite8(SELECT_RLD | RESET_BT_CT_CPT_S_IDX, &chan->control); - iowrite8(SELECT_RLD | RESET_E, &chan->control); + quad8_preset_register_set(priv, count->id, priv->preset[count->id]); spin_unlock_irqrestore(&priv->lock, irqflags); @@ -771,21 +781,6 @@ static int quad8_count_preset_read(struct counter_device *counter, return 0; } -static void quad8_preset_register_set(struct quad8 *const priv, const int id, - const unsigned int preset) -{ - struct channel_reg __iomem *const chan = priv->reg->channel + id; - int i; - - priv->preset[id] = preset; - - iowrite8(SELECT_RLD | RESET_BP, &chan->control); - - /* Set Preset Register */ - for (i = 0; i < 3; i++) - iowrite8(preset >> (8 * i), &chan->data); -} - static int quad8_count_preset_write(struct counter_device *counter, struct counter_count *count, u64 preset) { @@ -797,6 +792,7 @@ static int quad8_count_preset_write(struct counter_device *counter, spin_lock_irqsave(&priv->lock, irqflags); + priv->preset[count->id] = preset; quad8_preset_register_set(priv, count->id, preset); spin_unlock_irqrestore(&priv->lock, irqflags); @@ -843,6 +839,7 @@ static int quad8_count_ceiling_write(struct counter_device *counter, switch (u8_get_bits(priv->cmr[count->id], COUNT_MODE)) { case RANGE_LIMIT: case MODULO_N: + priv->preset[count->id] = ceiling; quad8_preset_register_set(priv, count->id, ceiling); spin_unlock_irqrestore(&priv->lock, irqflags); return 0; @@ -961,24 +958,28 @@ static int quad8_signal_fck_prescaler_read(struct counter_device *counter, return 0; } +static void quad8_filter_clock_prescaler_set(struct quad8 *const priv, const size_t id, + const u8 prescaler) +{ + struct channel_reg __iomem *const chan = priv->reg->channel + id; + + iowrite8(SELECT_RLD | RESET_BP, &chan->control); + iowrite8(prescaler, &chan->data); + iowrite8(SELECT_RLD | TRANSFER_PR0_TO_PSC, &chan->control); +} + static int quad8_signal_fck_prescaler_write(struct counter_device *counter, struct counter_signal *signal, u8 prescaler) { struct quad8 *const priv = counter_priv(counter); const size_t channel_id = signal->id / 2; - struct channel_reg __iomem *const chan = priv->reg->channel + channel_id; unsigned long irqflags; spin_lock_irqsave(&priv->lock, irqflags); priv->fck_prescaler[channel_id] = prescaler; - - iowrite8(SELECT_RLD | RESET_BP, &chan->control); - - /* Set filter clock factor */ - iowrite8(prescaler, &chan->data); - iowrite8(SELECT_RLD | RESET_BP | TRANSFER_PR0_TO_PSC, &chan->control); + quad8_filter_clock_prescaler_set(priv, channel_id, prescaler); spin_unlock_irqrestore(&priv->lock, irqflags); @@ -1178,18 +1179,10 @@ static irqreturn_t quad8_irq_handler(int irq, void *private) static void quad8_init_counter(struct quad8 *const priv, const size_t channel) { struct channel_reg __iomem *const chan = priv->reg->channel + channel; - unsigned long i; - iowrite8(SELECT_RLD | RESET_BP, &chan->control); - /* Reset filter clock factor */ - iowrite8(0, &chan->data); - iowrite8(SELECT_RLD | RESET_BP | TRANSFER_PR0_TO_PSC, &chan->control); - iowrite8(SELECT_RLD | RESET_BP, &chan->control); - /* Reset Preset Register */ - for (i = 0; i < 3; i++) - iowrite8(0x00, &chan->data); - iowrite8(SELECT_RLD | RESET_BT_CT_CPT_S_IDX, &chan->control); - iowrite8(SELECT_RLD | RESET_E, &chan->control); + quad8_filter_clock_prescaler_set(priv, channel, 0); + quad8_preset_register_set(priv, channel, 0); + quad8_flag_register_reset(priv, channel); /* Binary encoding; Normal count; non-quadrature mode */ priv->cmr[channel] = SELECT_CMR | BINARY | u8_encode_bits(NORMAL_COUNT, COUNT_MODE) | |