@@ -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)) {
@@ -351,15 +373,6 @@ static int wmt_i2c_probe(struct platform_device *pdev)
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,21 +381,13 @@ 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");
return err;
}
- err = i2c_add_adapter(adap);
- if (err)
- return err;
-
- platform_set_drvdata(pdev, i2c_dev);
-
- return 0;
+ return i2c_add_adapter(adap);
}
static void wmt_i2c_remove(struct platform_device *pdev)