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>

drivers/amba/bus.c

@@ -395,41 +395,20 @@ 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;
-
-	/*
-	 * Dynamically calculate the size of the resource
-	 * and use this for iomap
-	 */
-	size = resource_size(&dev->res);
-	tmp = ioremap(dev->res.start, size);
-	if (!tmp) {
-		ret = -ENOMEM;
-		goto err_release;
-	}
-
-	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;
+	if (ret)
+		goto err_pm;
 
 	/*
 	 * Find reset control(s) of the amba bus and de-assert them.
@@ -438,36 +417,36 @@ static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
 	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;
+			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_clk;
+	}
+
 	/*
 	 * 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);
+		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);
+		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;
+		dev->uci.devarch = readl(csbase + UCI_REG_DEVARCH_OFFSET);
+		dev->uci.devtype = readl(csbase + UCI_REG_DEVTYPE_OFFSET) & 0xff;
 	}
 
-		amba_put_disable_pclk(dev);
-
 	if (cid == AMBA_CID || cid == CORESIGHT_CID) {
 		dev->periphid = pid;
 		dev->cid = cid;
@@ -475,27 +454,39 @@ static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
 
 	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;
 }
 
 /*