static int amba_device_try_add()

in bus.c [397:498]

• 73 lines of code
• 16 McCabe index (conditional complexity)

static int amba_device_try_add(struct amba_device *dev, struct resource *parent)
{
	u32 size;
	void __iomem *tmp;
	int i, ret;

	ret = request_resource(parent, &dev->res);
	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;

		/*
		 * 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)
		goto err_release;

 skip_probe:
	ret = device_add(&dev->dev);
 err_release:
	if (ret)
		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;
}