mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-09-30 14:19:16 +00:00
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>
This commit is contained in:
parent
3cfb301997
commit
1f44de0f5e
1 changed files with 74 additions and 83 deletions
|
@ -395,12 +395,80 @@ static void amba_device_release(struct device *dev)
|
||||||
kfree(d);
|
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;
|
void __iomem *tmp;
|
||||||
int i, ret;
|
int i, ret;
|
||||||
|
|
||||||
|
ret = dev_pm_domain_attach(&dev->dev, true);
|
||||||
|
if (ret)
|
||||||
|
goto err_out;
|
||||||
|
|
||||||
|
ret = amba_get_enable_pclk(dev);
|
||||||
|
if (ret)
|
||||||
|
goto err_pm;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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_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);
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
ret = request_resource(parent, &dev->res);
|
||||||
if (ret)
|
if (ret)
|
||||||
goto err_out;
|
goto err_out;
|
||||||
|
@ -409,93 +477,16 @@ static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
|
||||||
if (dev->periphid != 0)
|
if (dev->periphid != 0)
|
||||||
goto skip_probe;
|
goto skip_probe;
|
||||||
|
|
||||||
/*
|
ret = amba_read_periphid(dev);
|
||||||
* 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;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
amba_put_disable_pclk(dev);
|
|
||||||
|
|
||||||
if (cid == AMBA_CID || cid == CORESIGHT_CID) {
|
|
||||||
dev->periphid = pid;
|
|
||||||
dev->cid = cid;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!dev->periphid)
|
|
||||||
ret = -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
iounmap(tmp);
|
|
||||||
dev_pm_domain_detach(&dev->dev, true);
|
|
||||||
|
|
||||||
if (ret)
|
if (ret)
|
||||||
goto err_release;
|
goto err_release;
|
||||||
|
skip_probe:
|
||||||
skip_probe:
|
|
||||||
ret = device_add(&dev->dev);
|
ret = device_add(&dev->dev);
|
||||||
err_release:
|
err_release:
|
||||||
if (ret)
|
if (ret)
|
||||||
release_resource(&dev->res);
|
release_resource(&dev->res);
|
||||||
err_out:
|
err_out:
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
err_reset:
|
|
||||||
amba_put_disable_pclk(dev);
|
|
||||||
iounmap(tmp);
|
|
||||||
dev_pm_domain_detach(&dev->dev, true);
|
|
||||||
goto err_release;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
Loading…
Reference in a new issue