irqchip/gicv3-its: Split its_alloc_tables() into two functions

The function is getting out of control, it has too many goto
statements and would be too complicated for adding a feature
two-level device table. So, it is time for us to cleanup and
move some of the logic to a separate function without affecting
the existing functionality.
Signed-off-by: NShanker Donthineni <shankerd@codeaurora.org>
Signed-off-by: NMarc Zyngier <marc.zyngier@arm.com>

drivers/irqchip/irq-gic-v3-its.c

@@ -56,13 +56,14 @@ struct its_collection {
 };
 
 /*
- * The ITS_BASER structure - contains memory information and cached
- * value of BASER register configuration.
+ * The ITS_BASER structure - contains memory information, cached
+ * value of BASER register configuration and ITS page size.
  */
 struct its_baser {
 	void		*base;
 	u64		val;
 	u32		order;
+	u32		psz;
 };
 
 /*
@@ -840,106 +841,35 @@ static void its_write_baser(struct its_node *its, struct its_baser *baser,
 	baser->val = its_read_baser(its, baser);
 }
 
-static void its_parse_baser_device(struct its_node *its, struct its_baser *baser,
-				   u32 *order)
-{
-	u64 esz = GITS_BASER_ENTRY_SIZE(its_read_baser(its, baser));
-	u32 ids = its->device_ids;
-	u32 new_order = *order;
-
-	/*
-	 * Allocate as many entries as required to fit the
-	 * range of device IDs that the ITS can grok... The ID
-	 * space being incredibly sparse, this results in a
-	 * massive waste of memory.
-	 */
-	new_order = max_t(u32, get_order(esz << ids), new_order);
-	if (new_order >= MAX_ORDER) {
-		new_order = MAX_ORDER - 1;
-		ids = ilog2(PAGE_ORDER_TO_SIZE(new_order) / esz);
-		pr_warn("ITS@%pa: Device Table too large, reduce ids %u->%u\n",
-			&its->phys_base, its->device_ids, ids);
-	}
-
-	*order = new_order;
-}
-
-static void its_free_tables(struct its_node *its)
+static int its_setup_baser(struct its_node *its, struct its_baser *baser,
+			   u64 cache, u64 shr, u32 psz, u32 order)
 {
-	int i;
-
-	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
-		if (its->tables[i].base) {
-			free_pages((unsigned long)its->tables[i].base,
-				   its->tables[i].order);
-			its->tables[i].base = NULL;
-		}
-	}
-}
-
-static int its_alloc_tables(const char *node_name, struct its_node *its)
-{
-	int err;
-	int i;
-	int psz = SZ_64K;
-	u64 shr = GITS_BASER_InnerShareable;
-	u64 cache;
-	u64 typer;
-	u32 ids;
-
-	if (its->flags & ITS_FLAGS_WORKAROUND_CAVIUM_22375) {
-		/*
-		 * erratum 22375: only alloc 8MB table size
-		 * erratum 24313: ignore memory access type
-		 */
-		cache	= 0;
-		ids	= 0x14;			/* 20 bits, 8MB */
-	} else {
-		cache	= GITS_BASER_WaWb;
-		typer	= readq_relaxed(its->base + GITS_TYPER);
-		ids	= GITS_TYPER_DEVBITS(typer);
-	}
-
-	its->device_ids = ids;
-
-	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
-		struct its_baser *baser = its->tables + i;
 	u64 val = its_read_baser(its, baser);
+	u64 esz = GITS_BASER_ENTRY_SIZE(val);
 	u64 type = GITS_BASER_TYPE(val);
-		u64 entry_size = GITS_BASER_ENTRY_SIZE(val);
-		int order = get_order(psz);
-		int alloc_pages;
-		u64 tmp;
+	u32 alloc_pages;
 	void *base;
-
-		if (type == GITS_BASER_TYPE_NONE)
-			continue;
-
-		if (type == GITS_BASER_TYPE_DEVICE)
-			its_parse_baser_device(its, baser, &order);
+	u64 tmp;
 
 retry_alloc_baser:
 	alloc_pages = (PAGE_ORDER_TO_SIZE(order) / psz);
 	if (alloc_pages > GITS_BASER_PAGES_MAX) {
+		pr_warn("ITS@%pa: %s too large, reduce ITS pages %u->%u\n",
+			&its->phys_base, its_base_type_string[type],
+			alloc_pages, GITS_BASER_PAGES_MAX);
 		alloc_pages = GITS_BASER_PAGES_MAX;
 		order = get_order(GITS_BASER_PAGES_MAX * psz);
-			pr_warn("%s: Device Table too large, reduce its page order to %u (%u pages)\n",
-				node_name, order, alloc_pages);
 	}
 
 	base = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, order);
-		if (!base) {
-			err = -ENOMEM;
-			goto out_free;
-		}
-
-		its->tables[i].base = base;
-		its->tables[i].order = order;
+	if (!base)
+		return -ENOMEM;
 
 retry_baser:
 	val = (virt_to_phys(base)				 |
 		(type << GITS_BASER_TYPE_SHIFT)			 |
-		       ((entry_size - 1) << GITS_BASER_ENTRY_SIZE_SHIFT) |
+		((esz - 1) << GITS_BASER_ENTRY_SIZE_SHIFT)	 |
+		((alloc_pages - 1) << GITS_BASER_PAGES_SHIFT)	 |
 		cache						 |
 		shr						 |
 		GITS_BASER_VALID);
@@ -956,9 +886,7 @@ static int its_alloc_tables(const char *node_name, struct its_node *its)
 		break;
 	}
 
-		val |= alloc_pages - 1;
-
-		its_write_baser_cache(its, baser, val);
+	its_write_baser(its, baser, val);
 	tmp = baser->val;
 
 	if ((val ^ tmp) & GITS_BASER_SHAREABILITY_MASK) {
@@ -984,7 +912,7 @@ static int its_alloc_tables(const char *node_name, struct its_node *its)
 		 * something is horribly wrong...
 		 */
 		free_pages((unsigned long)base, order);
-			its->tables[i].base = NULL;
+		baser->base = NULL;
 
 		switch (psz) {
 		case SZ_16K:
@@ -997,26 +925,108 @@ static int its_alloc_tables(const char *node_name, struct its_node *its)
 	}
 
 	if (val != tmp) {
-			pr_err("ITS: %s: GITS_BASER%d doesn't stick: %lx %lx\n",
-			       node_name, i,
+		pr_err("ITS@%pa: %s doesn't stick: %lx %lx\n",
+		       &its->phys_base, its_base_type_string[type],
 		       (unsigned long) val, (unsigned long) tmp);
-			err = -ENXIO;
-			goto out_free;
+		free_pages((unsigned long)base, order);
+		return -ENXIO;
 	}
 
-		pr_info("ITS: allocated %d %s @%lx (psz %dK, shr %d)\n",
-			(int)(PAGE_ORDER_TO_SIZE(order) / entry_size),
+	baser->order = order;
+	baser->base = base;
+	baser->psz = psz;
+
+	pr_info("ITS@%pa: allocated %d %s @%lx (psz %dK, shr %d)\n",
+		&its->phys_base, (int)(PAGE_ORDER_TO_SIZE(order) / esz),
 		its_base_type_string[type],
 		(unsigned long)virt_to_phys(base),
 		psz / SZ_1K, (int)shr >> GITS_BASER_SHAREABILITY_SHIFT);
-	}
 
 	return 0;
+}
 
-out_free:
-	its_free_tables(its);
+static void its_parse_baser_device(struct its_node *its, struct its_baser *baser,
+				   u32 *order)
+{
+	u64 esz = GITS_BASER_ENTRY_SIZE(its_read_baser(its, baser));
+	u32 ids = its->device_ids;
+	u32 new_order = *order;
+
+	/*
+	 * Allocate as many entries as required to fit the
+	 * range of device IDs that the ITS can grok... The ID
+	 * space being incredibly sparse, this results in a
+	 * massive waste of memory.
+	 */
+	new_order = max_t(u32, get_order(esz << ids), new_order);
+	if (new_order >= MAX_ORDER) {
+		new_order = MAX_ORDER - 1;
+		ids = ilog2(PAGE_ORDER_TO_SIZE(new_order) / esz);
+		pr_warn("ITS@%pa: Device Table too large, reduce ids %u->%u\n",
+			&its->phys_base, its->device_ids, ids);
+	}
+
+	*order = new_order;
+}
 
+static void its_free_tables(struct its_node *its)
+{
+	int i;
+
+	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
+		if (its->tables[i].base) {
+			free_pages((unsigned long)its->tables[i].base,
+				   its->tables[i].order);
+			its->tables[i].base = NULL;
+		}
+	}
+}
+
+static int its_alloc_tables(const char *node_name, struct its_node *its)
+{
+	u64 typer = readq_relaxed(its->base + GITS_TYPER);
+	u32 ids = GITS_TYPER_DEVBITS(typer);
+	u64 shr = GITS_BASER_InnerShareable;
+	u64 cache = GITS_BASER_WaWb;
+	u32 psz = SZ_64K;
+	int err, i;
+
+	if (its->flags & ITS_FLAGS_WORKAROUND_CAVIUM_22375) {
+		/*
+		* erratum 22375: only alloc 8MB table size
+		* erratum 24313: ignore memory access type
+		*/
+		cache   = GITS_BASER_nCnB;
+		ids     = 0x14;                 /* 20 bits, 8MB */
+	}
+
+	its->device_ids = ids;
+
+	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
+		struct its_baser *baser = its->tables + i;
+		u64 val = its_read_baser(its, baser);
+		u64 type = GITS_BASER_TYPE(val);
+		u32 order = get_order(psz);
+
+		if (type == GITS_BASER_TYPE_NONE)
+			continue;
+
+		if (type == GITS_BASER_TYPE_DEVICE)
+			its_parse_baser_device(its, baser, &order);
+
+		err = its_setup_baser(its, baser, cache, shr, psz, order);
+		if (err < 0) {
+			its_free_tables(its);
 			return err;
+		}
+
+		/* Update settings which will be used for next BASERn */
+		psz = baser->psz;
+		cache = baser->val & GITS_BASER_CACHEABILITY_MASK;
+		shr = baser->val & GITS_BASER_SHAREABILITY_MASK;
+	}
+
+	return 0;
 }
 
 static int its_alloc_collections(struct its_node *its)

include/linux/irqchip/arm-gic-v3.h

@@ -228,6 +228,7 @@
 #define GITS_BASER_PAGE_SIZE_64K	(2UL << GITS_BASER_PAGE_SIZE_SHIFT)
 #define GITS_BASER_PAGE_SIZE_MASK	(3UL << GITS_BASER_PAGE_SIZE_SHIFT)
 #define GITS_BASER_PAGES_MAX		256
+#define GITS_BASER_PAGES_SHIFT		(0)
 
 #define GITS_BASER_TYPE_NONE		0
 #define GITS_BASER_TYPE_DEVICE		1