ARM: 9193/1: amba: Add amba_read_periphid() helper

Add new amba_read_periphid() helper to simplify error handling.

Signed-off-by: Kefeng Wang <wangkefeng.wang@huawei.com>
Signed-off-by: Russell King (Oracle) <rmk+kernel@armlinux.org.uk>

diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index d3bd14a..0073d8b 100644 (file)
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -395,107 +395,98 @@ static void amba_device_release(struct device *dev)
        kfree(d);
 }
 
-static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
+static int amba_read_periphid(struct amba_device *dev)
 {
-       u32 size;
+       struct reset_control *rstc;
+       u32 size, pid, cid;
        void __iomem *tmp;
        int i, ret;
 
-       ret = request_resource(parent, &dev->res);
+       ret = dev_pm_domain_attach(&dev->dev, true);
        if (ret)
                goto err_out;
 
-       /* Hard-coded primecell ID instead of plug-n-play */
-       if (dev->periphid != 0)
-               goto skip_probe;
+       ret = amba_get_enable_pclk(dev);
+       if (ret)
+               goto err_pm;
 
        /*
-        * Dynamically calculate the size of the resource
-        * and use this for iomap
+        * Find reset control(s) of the amba bus and de-assert them.
         */
+       rstc = of_reset_control_array_get_optional_shared(dev->dev.of_node);
+       if (IS_ERR(rstc)) {
+               ret = PTR_ERR(rstc);
+               if (ret != -EPROBE_DEFER)
+                       dev_err(&dev->dev, "can't get reset: %d\n", ret);
+               goto err_clk;
+       }
+       reset_control_deassert(rstc);
+       reset_control_put(rstc);
+
        size = resource_size(&dev->res);
        tmp = ioremap(dev->res.start, size);
        if (!tmp) {
                ret = -ENOMEM;
-               goto err_release;
+               goto err_clk;
        }
 
-       ret = dev_pm_domain_attach(&dev->dev, true);
-       if (ret) {
-               iounmap(tmp);
-               goto err_release;
-       }
-
-       ret = amba_get_enable_pclk(dev);
-       if (ret == 0) {
-               u32 pid, cid;
-               struct reset_control *rstc;
-
-               /*
-                * Find reset control(s) of the amba bus and de-assert them.
-                */
-               rstc = of_reset_control_array_get_optional_shared(dev->dev.of_node);
-               if (IS_ERR(rstc)) {
-                       ret = PTR_ERR(rstc);
-                       if (ret != -EPROBE_DEFER)
-                               dev_err(&dev->dev, "can't get reset: %d\n",
-                                       ret);
-                       goto err_reset;
-               }
-               reset_control_deassert(rstc);
-               reset_control_put(rstc);
-
-               /*
-                * Read pid and cid based on size of resource
-                * they are located at end of region
-                */
-               for (pid = 0, i = 0; i < 4; i++)
-                       pid |= (readl(tmp + size - 0x20 + 4 * i) & 255) <<
-                               (i * 8);
-               for (cid = 0, i = 0; i < 4; i++)
-                       cid |= (readl(tmp + size - 0x10 + 4 * i) & 255) <<
-                               (i * 8);
-
-               if (cid == CORESIGHT_CID) {
-                       /* set the base to the start of the last 4k block */
-                       void __iomem *csbase = tmp + size - 4096;
-
-                       dev->uci.devarch =
-                               readl(csbase + UCI_REG_DEVARCH_OFFSET);
-                       dev->uci.devtype =
-                               readl(csbase + UCI_REG_DEVTYPE_OFFSET) & 0xff;
-               }
+       /*
+        * Read pid and cid based on size of resource
+        * they are located at end of region
+        */
+       for (pid = 0, i = 0; i < 4; i++)
+               pid |= (readl(tmp + size - 0x20 + 4 * i) & 255) << (i * 8);
+       for (cid = 0, i = 0; i < 4; i++)
+               cid |= (readl(tmp + size - 0x10 + 4 * i) & 255) << (i * 8);
 
-               amba_put_disable_pclk(dev);
+       if (cid == CORESIGHT_CID) {
+               /* set the base to the start of the last 4k block */
+               void __iomem *csbase = tmp + size - 4096;
 
-               if (cid == AMBA_CID || cid == CORESIGHT_CID) {
-                       dev->periphid = pid;
-                       dev->cid = cid;
-               }
+               dev->uci.devarch = readl(csbase + UCI_REG_DEVARCH_OFFSET);
+               dev->uci.devtype = readl(csbase + UCI_REG_DEVTYPE_OFFSET) & 0xff;
+       }
 
-               if (!dev->periphid)
-                       ret = -ENODEV;
+       if (cid == AMBA_CID || cid == CORESIGHT_CID) {
+               dev->periphid = pid;
+               dev->cid = cid;
        }
 
+       if (!dev->periphid)
+               ret = -ENODEV;
+
        iounmap(tmp);
+
+err_clk:
+       amba_put_disable_pclk(dev);
+err_pm:
        dev_pm_domain_detach(&dev->dev, true);
+err_out:
+       return ret;
+}
 
+static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
+{
+       int ret;
+
+       ret = request_resource(parent, &dev->res);
        if (ret)
-               goto err_release;
+               goto err_out;
+
+       /* Hard-coded primecell ID instead of plug-n-play */
+       if (dev->periphid != 0)
+               goto skip_probe;
 
- skip_probe:
+       ret = amba_read_periphid(dev);
+       if (ret)
+               goto err_release;
+skip_probe:
        ret = device_add(&dev->dev);
- err_release:
+err_release:
        if (ret)
                release_resource(&dev->res);
- err_out:
+err_out:
        return ret;
-
- err_reset:
-       amba_put_disable_pclk(dev);
-       iounmap(tmp);
-       dev_pm_domain_detach(&dev->dev, true);
-       goto err_release;
 }
 
 /*