diff options
| -rw-r--r-- | drivers/i2c/busses/i2c-wmt.c | 65 |
1 files changed, 37 insertions, 28 deletions
diff --git a/drivers/i2c/busses/i2c-wmt.c b/drivers/i2c/busses/i2c-wmt.c index 198afee5233c..76ede433f65c 100644 --- a/drivers/i2c/busses/i2c-wmt.c +++ b/drivers/i2c/busses/i2c-wmt.c @@ -109,7 +109,7 @@ static int wmt_check_status(struct wmt_i2c_dev *i2c_dev) unsigned long wait_result; wait_result = wait_for_completion_timeout(&i2c_dev->complete, - msecs_to_jiffies(500)); + msecs_to_jiffies(500)); if (!wait_result) return -ETIMEDOUT; @@ -286,6 +286,38 @@ static irqreturn_t wmt_i2c_isr(int irq, void *data) return IRQ_HANDLED; } +static int wmt_i2c_init(struct platform_device *pdev, struct wmt_i2c_dev **pi2c_dev) +{ + int err; + struct wmt_i2c_dev *i2c_dev; + struct device_node *np = pdev->dev.of_node; + + i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL); + if (!i2c_dev) + return -ENOMEM; + + i2c_dev->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL); + if (IS_ERR(i2c_dev->base)) + return PTR_ERR(i2c_dev->base); + + i2c_dev->irq = irq_of_parse_and_map(np, 0); + if (!i2c_dev->irq) + return -EINVAL; + + err = devm_request_irq(&pdev->dev, i2c_dev->irq, wmt_i2c_isr, + 0, pdev->name, i2c_dev); + if (err) + return dev_err_probe(&pdev->dev, err, + "failed to request irq %i\n", i2c_dev->irq); + + i2c_dev->dev = &pdev->dev; + init_completion(&i2c_dev->complete); + platform_set_drvdata(pdev, i2c_dev); + + *pi2c_dev = i2c_dev; + return 0; +} + static int wmt_i2c_reset_hardware(struct wmt_i2c_dev *i2c_dev) { int err; @@ -327,19 +359,9 @@ static int wmt_i2c_probe(struct platform_device *pdev) int err; u32 clk_rate; - i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL); - if (!i2c_dev) - return -ENOMEM; - - i2c_dev->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL); - if (IS_ERR(i2c_dev->base)) - return PTR_ERR(i2c_dev->base); - - i2c_dev->irq = irq_of_parse_and_map(np, 0); - if (!i2c_dev->irq) { - dev_err(&pdev->dev, "irq missing or invalid\n"); - return -EINVAL; - } + err = wmt_i2c_init(pdev, &i2c_dev); + if (err) + return err; i2c_dev->clk = of_clk_get(np, 0); if (IS_ERR(i2c_dev->clk)) { @@ -348,18 +370,9 @@ static int wmt_i2c_probe(struct platform_device *pdev) } err = of_property_read_u32(np, "clock-frequency", &clk_rate); - if (!err && (clk_rate == I2C_MAX_FAST_MODE_FREQ)) + if (!err && clk_rate == I2C_MAX_FAST_MODE_FREQ) i2c_dev->tcr = TCR_FAST_MODE; - i2c_dev->dev = &pdev->dev; - - err = devm_request_irq(&pdev->dev, i2c_dev->irq, wmt_i2c_isr, 0, - "i2c", i2c_dev); - if (err) { - dev_err(&pdev->dev, "failed to request irq %i\n", i2c_dev->irq); - return err; - } - adap = &i2c_dev->adapter; i2c_set_adapdata(adap, i2c_dev); strscpy(adap->name, "WMT I2C adapter", sizeof(adap->name)); @@ -368,8 +381,6 @@ static int wmt_i2c_probe(struct platform_device *pdev) adap->dev.parent = &pdev->dev; adap->dev.of_node = pdev->dev.of_node; - init_completion(&i2c_dev->complete); - err = wmt_i2c_reset_hardware(i2c_dev); if (err) { dev_err(&pdev->dev, "error initializing hardware\n"); @@ -380,8 +391,6 @@ static int wmt_i2c_probe(struct platform_device *pdev) if (err) goto err_disable_clk; - platform_set_drvdata(pdev, i2c_dev); - return 0; err_disable_clk: |