static int __init its_probe_one()

in irq-gic-v3-its.c [4961:5115]

• 125 lines of code
• 19 McCabe index (conditional complexity)

static int __init its_probe_one(struct resource *res,
				struct fwnode_handle *handle, int numa_node)
{
	struct its_node *its;
	void __iomem *its_base;
	u32 val, ctlr;
	u64 baser, tmp, typer;
	struct page *page;
	int err;

	its_base = ioremap(res->start, SZ_64K);
	if (!its_base) {
		pr_warn("ITS@%pa: Unable to map ITS registers\n", &res->start);
		return -ENOMEM;
	}

	val = readl_relaxed(its_base + GITS_PIDR2) & GIC_PIDR2_ARCH_MASK;
	if (val != 0x30 && val != 0x40) {
		pr_warn("ITS@%pa: No ITS detected, giving up\n", &res->start);
		err = -ENODEV;
		goto out_unmap;
	}

	err = its_force_quiescent(its_base);
	if (err) {
		pr_warn("ITS@%pa: Failed to quiesce, giving up\n", &res->start);
		goto out_unmap;
	}

	pr_info("ITS %pR\n", res);

	its = kzalloc(sizeof(*its), GFP_KERNEL);
	if (!its) {
		err = -ENOMEM;
		goto out_unmap;
	}

	raw_spin_lock_init(&its->lock);
	mutex_init(&its->dev_alloc_lock);
	INIT_LIST_HEAD(&its->entry);
	INIT_LIST_HEAD(&its->its_device_list);
	typer = gic_read_typer(its_base + GITS_TYPER);
	its->typer = typer;
	its->base = its_base;
	its->phys_base = res->start;
	if (is_v4(its)) {
		if (!(typer & GITS_TYPER_VMOVP)) {
			err = its_compute_its_list_map(res, its_base);
			if (err < 0)
				goto out_free_its;

			its->list_nr = err;

			pr_info("ITS@%pa: Using ITS number %d\n",
				&res->start, err);
		} else {
			pr_info("ITS@%pa: Single VMOVP capable\n", &res->start);
		}

		if (is_v4_1(its)) {
			u32 svpet = FIELD_GET(GITS_TYPER_SVPET, typer);

			its->sgir_base = ioremap(res->start + SZ_128K, SZ_64K);
			if (!its->sgir_base) {
				err = -ENOMEM;
				goto out_free_its;
			}

			its->mpidr = readl_relaxed(its_base + GITS_MPIDR);

			pr_info("ITS@%pa: Using GICv4.1 mode %08x %08x\n",
				&res->start, its->mpidr, svpet);
		}
	}

	its->numa_node = numa_node;

	page = alloc_pages_node(its->numa_node, GFP_KERNEL | __GFP_ZERO,
				get_order(ITS_CMD_QUEUE_SZ));
	if (!page) {
		err = -ENOMEM;
		goto out_unmap_sgir;
	}
	its->cmd_base = (void *)page_address(page);
	its->cmd_write = its->cmd_base;
	its->fwnode_handle = handle;
	its->get_msi_base = its_irq_get_msi_base;
	its->msi_domain_flags = IRQ_DOMAIN_FLAG_MSI_REMAP;

	its_enable_quirks(its);

	err = its_alloc_tables(its);
	if (err)
		goto out_free_cmd;

	err = its_alloc_collections(its);
	if (err)
		goto out_free_tables;

	baser = (virt_to_phys(its->cmd_base)	|
		 GITS_CBASER_RaWaWb		|
		 GITS_CBASER_InnerShareable	|
		 (ITS_CMD_QUEUE_SZ / SZ_4K - 1)	|
		 GITS_CBASER_VALID);

	gits_write_cbaser(baser, its->base + GITS_CBASER);
	tmp = gits_read_cbaser(its->base + GITS_CBASER);

	if ((tmp ^ baser) & GITS_CBASER_SHAREABILITY_MASK) {
		if (!(tmp & GITS_CBASER_SHAREABILITY_MASK)) {
			/*
			 * The HW reports non-shareable, we must
			 * remove the cacheability attributes as
			 * well.
			 */
			baser &= ~(GITS_CBASER_SHAREABILITY_MASK |
				   GITS_CBASER_CACHEABILITY_MASK);
			baser |= GITS_CBASER_nC;
			gits_write_cbaser(baser, its->base + GITS_CBASER);
		}
		pr_info("ITS: using cache flushing for cmd queue\n");
		its->flags |= ITS_FLAGS_CMDQ_NEEDS_FLUSHING;
	}

	gits_write_cwriter(0, its->base + GITS_CWRITER);
	ctlr = readl_relaxed(its->base + GITS_CTLR);
	ctlr |= GITS_CTLR_ENABLE;
	if (is_v4(its))
		ctlr |= GITS_CTLR_ImDe;
	writel_relaxed(ctlr, its->base + GITS_CTLR);

	err = its_init_domain(handle, its);
	if (err)
		goto out_free_tables;

	raw_spin_lock(&its_lock);
	list_add(&its->entry, &its_nodes);
	raw_spin_unlock(&its_lock);

	return 0;

out_free_tables:
	its_free_tables(its);
out_free_cmd:
	free_pages((unsigned long)its->cmd_base, get_order(ITS_CMD_QUEUE_SZ));
out_unmap_sgir:
	if (its->sgir_base)
		iounmap(its->sgir_base);
out_free_its:
	kfree(its);
out_unmap:
	iounmap(its_base);
	pr_err("ITS@%pa: failed probing (%d)\n", &res->start, err);
	return err;
}