From 0118681b2bc2f182b7cd4c6569632cf2729cc73e Mon Sep 17 00:00:00 2001 From: Jens Taprogge Date: Wed, 12 Sep 2012 14:55:23 +0200 Subject: [PATCH] Staging: ipack/bridges/tpci200: add helpers for writing control regs. Convert tpci200_set_clockrate and tpci200_interrupt. Signed-off-by: Jens Taprogge Signed-off-by: Samuel Iglesias Gonsalvez Signed-off-by: Greg Kroah-Hartman --- drivers/staging/ipack/bridges/tpci200.c | 49 ++++++++++++++++--------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/drivers/staging/ipack/bridges/tpci200.c b/drivers/staging/ipack/bridges/tpci200.c index 9d886b7fdfc..25605196dba 100644 --- a/drivers/staging/ipack/bridges/tpci200.c +++ b/drivers/staging/ipack/bridges/tpci200.c @@ -53,6 +53,32 @@ static struct tpci200_board *check_slot(struct ipack_device *dev) return tpci200; } +static void __tpci200_clear_mask(__le16 __iomem *addr, u16 mask) +{ + iowrite16(ioread16(addr) & (~mask), addr); +} + +static void tpci200_clear_mask(struct tpci200_board *tpci200, + __le16 __iomem *addr, u16 mask) +{ + mutex_lock(&tpci200->mutex); + __tpci200_clear_mask(addr, mask); + mutex_unlock(&tpci200->mutex); +} + +static void __tpci200_set_mask(__le16 __iomem *addr, u16 mask) +{ + iowrite16(ioread16(addr) | mask, addr); +} + +static void tpci200_set_mask(struct tpci200_board *tpci200, + __le16 __iomem *addr, u16 mask) +{ + mutex_lock(&tpci200->mutex); + __tpci200_set_mask(addr, mask); + mutex_unlock(&tpci200->mutex); +} + static void tpci200_unregister(struct tpci200_board *tpci200) { int i; @@ -86,7 +112,7 @@ static irqreturn_t tpci200_interrupt(int irq, void *dev_id) { struct tpci200_board *tpci200 = (struct tpci200_board *) dev_id; int i; - unsigned short status_reg, reg_value; + unsigned short status_reg; unsigned short unhandled_ints = 0; irqreturn_t ret = IRQ_NONE; struct slot_irq *slot_irq; @@ -124,12 +150,9 @@ static irqreturn_t tpci200_interrupt(int irq, void *dev_id) dev_info(&slot_irq->holder->dev, "No registered ISR for slot [%d:%d]!. IRQ will be disabled.\n", tpci200->number, i); - reg_value = readw( - &tpci200->info->interface_regs->control[i]); - reg_value &= - ~(TPCI200_INT0_EN | TPCI200_INT1_EN); - writew(reg_value, - &tpci200->info->interface_regs->control[i]); + __tpci200_clear_mask( + &tpci200->info->interface_regs->control[i], + TPCI200_INT0_EN | TPCI200_INT1_EN); } } } @@ -518,30 +541,22 @@ static int tpci200_set_clockrate(struct ipack_device *dev, int mherz) { struct tpci200_board *tpci200 = check_slot(dev); u16 __iomem *addr; - u16 reg; if (!tpci200) return -ENODEV; addr = &tpci200->info->interface_regs->control[dev->slot]; - /* Ensure the control register is not changed by another task after we - * have read it. */ - mutex_lock(&tpci200->mutex); - reg = ioread16(addr); switch (mherz) { case 8: - reg &= ~(TPCI200_CLK32); + tpci200_clear_mask(tpci200, addr, TPCI200_CLK32); break; case 32: - reg |= TPCI200_CLK32; + tpci200_set_mask(tpci200, addr, TPCI200_CLK32); break; default: - mutex_unlock(&tpci200->mutex); return -EINVAL; } - iowrite16(reg, addr); - mutex_unlock(&tpci200->mutex); return 0; } -- 2.20.1