diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c index fbeae872ece1..2d18a35dc18c 100644 --- a/drivers/i2c/busses/i2c-tegra.c +++ b/drivers/i2c/busses/i2c-tegra.c @@ -514,10 +514,24 @@ static void tegra_i2c_vi_init(struct tegra_i2c_dev *i2c_dev) i2c_writel(i2c_dev, 0x0, I2C_TLOW_SEXT); } +static int tegra_i2c_poll_register(struct tegra_i2c_dev *i2c_dev, + u32 reg, u32 mask, u32 delay_us, + u32 timeout_us) +{ + void __iomem *addr = i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg); + u32 val; + + if (!i2c_dev->is_curr_atomic_xfer) + return readl_relaxed_poll_timeout(addr, val, !(val & mask), + delay_us, timeout_us); + + return readl_relaxed_poll_timeout_atomic(addr, val, !(val & mask), + delay_us, timeout_us); +} + static int tegra_i2c_flush_fifos(struct tegra_i2c_dev *i2c_dev) { - u32 mask, val, offset, reg_offset; - void __iomem *addr; + u32 mask, val, offset; int err; if (i2c_dev->hw->has_mst_fifo) { @@ -534,16 +548,7 @@ static int tegra_i2c_flush_fifos(struct tegra_i2c_dev *i2c_dev) val |= mask; i2c_writel(i2c_dev, val, offset); - reg_offset = tegra_i2c_reg_addr(i2c_dev, offset); - addr = i2c_dev->base + reg_offset; - - if (i2c_dev->is_curr_atomic_xfer) - err = readl_relaxed_poll_timeout_atomic(addr, val, !(val & mask), - 1000, 1000000); - else - err = readl_relaxed_poll_timeout(addr, val, !(val & mask), - 1000, 1000000); - + err = tegra_i2c_poll_register(i2c_dev, offset, mask, 1000, 1000000); if (err) { dev_err(i2c_dev->dev, "failed to flush FIFO\n"); return err; @@ -553,30 +558,18 @@ static int tegra_i2c_flush_fifos(struct tegra_i2c_dev *i2c_dev) static int tegra_i2c_wait_for_config_load(struct tegra_i2c_dev *i2c_dev) { - unsigned long reg_offset; - void __iomem *addr; - u32 val; int err; - if (i2c_dev->hw->has_config_load_reg) { - reg_offset = tegra_i2c_reg_addr(i2c_dev, I2C_CONFIG_LOAD); - addr = i2c_dev->base + reg_offset; - i2c_writel(i2c_dev, I2C_MSTR_CONFIG_LOAD, I2C_CONFIG_LOAD); + if (!i2c_dev->hw->has_config_load_reg) + return 0; - if (i2c_dev->is_curr_atomic_xfer) - err = readl_relaxed_poll_timeout_atomic( - addr, val, val == 0, 1000, - I2C_CONFIG_LOAD_TIMEOUT); - else - err = readl_relaxed_poll_timeout( - addr, val, val == 0, 1000, - I2C_CONFIG_LOAD_TIMEOUT); + i2c_writel(i2c_dev, I2C_MSTR_CONFIG_LOAD, I2C_CONFIG_LOAD); - if (err) { - dev_warn(i2c_dev->dev, - "timeout waiting for config load\n"); - return err; - } + err = tegra_i2c_poll_register(i2c_dev, I2C_CONFIG_LOAD, 0xffffffff, + 1000, I2C_CONFIG_LOAD_TIMEOUT); + if (err) { + dev_warn(i2c_dev->dev, "timeout waiting for config load\n"); + return err; } return 0;