Commit 1f44de0f authored by Wang Kefeng's avatar Wang Kefeng Committed by Russell King (Oracle)
Browse files

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



Add new amba_read_periphid() helper to simplify error handling.

Signed-off-by: default avatarKefeng Wang <wangkefeng.wang@huawei.com>
Signed-off-by: default avatarRussell King (Oracle) <rmk+kernel@armlinux.org.uk>
parent 3cfb3019

drivers/amba/bus.c

+62 −71
Original line number Diff line number Diff line
@@ -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,14 +454,32 @@ 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;



	ret = amba_read_periphid(dev);

	if (ret)

		goto err_release;

skip_probe:

	ret = device_add(&dev->dev);

err_release:
@@ -490,12 +487,6 @@ static int amba_device_try_add(struct amba_device *dev, struct resource *parent) 
		release_resource(&dev->res);

err_out:

	return ret;



 err_reset:

	amba_put_disable_pclk(dev);

	iounmap(tmp);

	dev_pm_domain_detach(&dev->dev, true);

	goto err_release;

}



/*