renegade-project/linux-next
2
0
Fork 0
mirror of https://github.com/edk2-porting/linux-next.git synced 2024-12-04 03:14:02 +08:00
Code

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

Browse Source 
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:
Wang Kefeng 2022-04-18 07:07:13 +01:00 committed by Russell King (Oracle)
parent 3cfb301997
commit 1f44de0f5e
1 changed files with 74 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
drivers/amba/bus.c
View File

@ -395,12 +395,80 @@ 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 = 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);
if (ret)
goto err_out;
@ -409,93 +477,16 @@ static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
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;
/*
* 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);
ret = amba_read_periphid(dev);
if (ret)
goto err_release;
skip_probe:
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;
}
/*