static int create_csd()

in fsl_pamu.c [580:708]

• 97 lines of code
• 22 McCabe index (conditional complexity)

static int create_csd(phys_addr_t phys, size_t size, u32 csd_port_id)
{
	struct device_node *np;
	const __be32 *iprop;
	void __iomem *lac = NULL;	/* Local Access Control registers */
	struct ccsr_law __iomem *law;
	void __iomem *ccm = NULL;
	u32 __iomem *csdids;
	unsigned int i, num_laws, num_csds;
	u32 law_target = 0;
	u32 csd_id = 0;
	int ret = 0;

	np = of_find_compatible_node(NULL, NULL, "fsl,corenet-law");
	if (!np)
		return -ENODEV;

	iprop = of_get_property(np, "fsl,num-laws", NULL);
	if (!iprop) {
		ret = -ENODEV;
		goto error;
	}

	num_laws = be32_to_cpup(iprop);
	if (!num_laws) {
		ret = -ENODEV;
		goto error;
	}

	lac = of_iomap(np, 0);
	if (!lac) {
		ret = -ENODEV;
		goto error;
	}

	/* LAW registers are at offset 0xC00 */
	law = lac + 0xC00;

	of_node_put(np);

	np = of_find_compatible_node(NULL, NULL, "fsl,corenet-cf");
	if (!np) {
		ret = -ENODEV;
		goto error;
	}

	iprop = of_get_property(np, "fsl,ccf-num-csdids", NULL);
	if (!iprop) {
		ret = -ENODEV;
		goto error;
	}

	num_csds = be32_to_cpup(iprop);
	if (!num_csds) {
		ret = -ENODEV;
		goto error;
	}

	ccm = of_iomap(np, 0);
	if (!ccm) {
		ret = -ENOMEM;
		goto error;
	}

	/* The undocumented CSDID registers are at offset 0x600 */
	csdids = ccm + 0x600;

	of_node_put(np);
	np = NULL;

	/* Find an unused coherence subdomain ID */
	for (csd_id = 0; csd_id < num_csds; csd_id++) {
		if (!csdids[csd_id])
			break;
	}

	/* Store the Port ID in the (undocumented) proper CIDMRxx register */
	csdids[csd_id] = csd_port_id;

	/* Find the DDR LAW that maps to our buffer. */
	for (i = 0; i < num_laws; i++) {
		if (law[i].lawar & LAWAR_EN) {
			phys_addr_t law_start, law_end;

			law_start = make64(law[i].lawbarh, law[i].lawbarl);
			law_end = law_start +
				(2ULL << (law[i].lawar & LAWAR_SIZE_MASK));

			if (law_start <= phys && phys < law_end) {
				law_target = law[i].lawar & LAWAR_TARGET_MASK;
				break;
			}
		}
	}

	if (i == 0 || i == num_laws) {
		/* This should never happen */
		ret = -ENOENT;
		goto error;
	}

	/* Find a free LAW entry */
	while (law[--i].lawar & LAWAR_EN) {
		if (i == 0) {
			/* No higher priority LAW slots available */
			ret = -ENOENT;
			goto error;
		}
	}

	law[i].lawbarh = upper_32_bits(phys);
	law[i].lawbarl = lower_32_bits(phys);
	wmb();
	law[i].lawar = LAWAR_EN | law_target | (csd_id << LAWAR_CSDID_SHIFT) |
		(LAW_SIZE_4K + get_order(size));
	wmb();

error:
	if (ccm)
		iounmap(ccm);

	if (lac)
		iounmap(lac);

	if (np)
		of_node_put(np);

	return ret;
}