static int allocate_vpe_l1_table()

in irq-gic-v3-its.c [2800:2921]

• 76 lines of code
• 11 McCabe index (conditional complexity)

static int allocate_vpe_l1_table(void)
{
	void __iomem *vlpi_base = gic_data_rdist_vlpi_base();
	u64 val, gpsz, npg, pa;
	unsigned int psz = SZ_64K;
	unsigned int np, epp, esz;
	struct page *page;

	if (!gic_rdists->has_rvpeid)
		return 0;

	/*
	 * if VPENDBASER.Valid is set, disable any previously programmed
	 * VPE by setting PendingLast while clearing Valid. This has the
	 * effect of making sure no doorbell will be generated and we can
	 * then safely clear VPROPBASER.Valid.
	 */
	if (gicr_read_vpendbaser(vlpi_base + GICR_VPENDBASER) & GICR_VPENDBASER_Valid)
		gicr_write_vpendbaser(GICR_VPENDBASER_PendingLast,
				      vlpi_base + GICR_VPENDBASER);

	/*
	 * If we can inherit the configuration from another RD, let's do
	 * so. Otherwise, we have to go through the allocation process. We
	 * assume that all RDs have the exact same requirements, as
	 * nothing will work otherwise.
	 */
	val = inherit_vpe_l1_table_from_rd(&gic_data_rdist()->vpe_table_mask);
	if (val & GICR_VPROPBASER_4_1_VALID)
		goto out;

	gic_data_rdist()->vpe_table_mask = kzalloc(sizeof(cpumask_t), GFP_ATOMIC);
	if (!gic_data_rdist()->vpe_table_mask)
		return -ENOMEM;

	val = inherit_vpe_l1_table_from_its();
	if (val & GICR_VPROPBASER_4_1_VALID)
		goto out;

	/* First probe the page size */
	val = FIELD_PREP(GICR_VPROPBASER_4_1_PAGE_SIZE, GIC_PAGE_SIZE_64K);
	gicr_write_vpropbaser(val, vlpi_base + GICR_VPROPBASER);
	val = gicr_read_vpropbaser(vlpi_base + GICR_VPROPBASER);
	gpsz = FIELD_GET(GICR_VPROPBASER_4_1_PAGE_SIZE, val);
	esz = FIELD_GET(GICR_VPROPBASER_4_1_ENTRY_SIZE, val);

	switch (gpsz) {
	default:
		gpsz = GIC_PAGE_SIZE_4K;
		fallthrough;
	case GIC_PAGE_SIZE_4K:
		psz = SZ_4K;
		break;
	case GIC_PAGE_SIZE_16K:
		psz = SZ_16K;
		break;
	case GIC_PAGE_SIZE_64K:
		psz = SZ_64K;
		break;
	}

	/*
	 * Start populating the register from scratch, including RO fields
	 * (which we want to print in debug cases...)
	 */
	val = 0;
	val |= FIELD_PREP(GICR_VPROPBASER_4_1_PAGE_SIZE, gpsz);
	val |= FIELD_PREP(GICR_VPROPBASER_4_1_ENTRY_SIZE, esz);

	/* How many entries per GIC page? */
	esz++;
	epp = psz / (esz * SZ_8);

	/*
	 * If we need more than just a single L1 page, flag the table
	 * as indirect and compute the number of required L1 pages.
	 */
	if (epp < ITS_MAX_VPEID) {
		int nl2;

		val |= GICR_VPROPBASER_4_1_INDIRECT;

		/* Number of L2 pages required to cover the VPEID space */
		nl2 = DIV_ROUND_UP(ITS_MAX_VPEID, epp);

		/* Number of L1 pages to point to the L2 pages */
		npg = DIV_ROUND_UP(nl2 * SZ_8, psz);
	} else {
		npg = 1;
	}

	val |= FIELD_PREP(GICR_VPROPBASER_4_1_SIZE, npg - 1);

	/* Right, that's the number of CPU pages we need for L1 */
	np = DIV_ROUND_UP(npg * psz, PAGE_SIZE);

	pr_debug("np = %d, npg = %lld, psz = %d, epp = %d, esz = %d\n",
		 np, npg, psz, epp, esz);
	page = alloc_pages(GFP_ATOMIC | __GFP_ZERO, get_order(np * PAGE_SIZE));
	if (!page)
		return -ENOMEM;

	gic_data_rdist()->vpe_l1_base = page_address(page);
	pa = virt_to_phys(page_address(page));
	WARN_ON(!IS_ALIGNED(pa, psz));

	val |= FIELD_PREP(GICR_VPROPBASER_4_1_ADDR, pa >> 12);
	val |= GICR_VPROPBASER_RaWb;
	val |= GICR_VPROPBASER_InnerShareable;
	val |= GICR_VPROPBASER_4_1_Z;
	val |= GICR_VPROPBASER_4_1_VALID;

out:
	gicr_write_vpropbaser(val, vlpi_base + GICR_VPROPBASER);
	cpumask_set_cpu(smp_processor_id(), gic_data_rdist()->vpe_table_mask);

	pr_debug("CPU%d: VPROPBASER = %llx %*pbl\n",
		 smp_processor_id(), val,
		 cpumask_pr_args(gic_data_rdist()->vpe_table_mask));

	return 0;
}