[PATCH 1/3] irqchip/gicv3-its: Adjust the implementation of its_alloc_tables

[Zhen Lei <thunder.leizhen@huawei.com>]

For the old version, there maybe some faults:
1. For each Interrupt Collection table, 4K size is enough. But now the
   default is 64K(if "Page Size" field is not read-only).
2. Suppose two read-only "Page Size" table exist, and software found 4K
   first, then 16K. For the first time, the value of local variable "psz"
   will drop to 4K, but we have not reinitialize it again in the loop.
   So, further process 16K read-only "Page Size" will report error.

In this patch, detect all the read-only fields first. So that, no need
to try over and over again.

Signed-off-by: Zhen Lei <thunder.leizhen@huawei.com>
---
 drivers/irqchip/irq-gic-v3-its.c | 105 +++++++++++++++++++++++----------------
 1 file changed, 62 insertions(+), 43 deletions(-)

diff --git a/drivers/irqchip/irq-gic-v3-its.c b/drivers/irqchip/irq-gic-v3-its.c
index 9687f8a..2577f06 100644
--- a/drivers/irqchip/irq-gic-v3-its.c
+++ b/drivers/irqchip/irq-gic-v3-its.c
@@ -800,20 +800,21 @@ static int its_alloc_tables(struct its_node *its)
 {
 	int err;
 	int i;
-	int psz = SZ_64K;
-	u64 shr = GITS_BASER_InnerShareable;
-	u64 cache = GITS_BASER_WaWb;
+	int psz;
+	u64 shr;
+	u64 cache;

 	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
 		u64 val = readq_relaxed(its->base + GITS_BASER + i * 8);
 		u64 type = GITS_BASER_TYPE(val);
 		u64 entry_size = GITS_BASER_ENTRY_SIZE(val);
-		int order = get_order(psz);
+		int order;
 		int alloc_size;
 		u64 tmp;
 		void *base;

-		if (type == GITS_BASER_TYPE_NONE)
+		switch (type) {
+		case GITS_BASER_TYPE_NONE:
 			continue;

 		/*
@@ -824,16 +825,65 @@ static int its_alloc_tables(struct its_node *its)
 		 *
 		 * For other tables, only allocate a single page.
 		 */
-		if (type == GITS_BASER_TYPE_DEVICE) {
+		case GITS_BASER_TYPE_DEVICE: {
 			u64 typer = readq_relaxed(its->base + GITS_TYPER);
 			u32 ids = GITS_TYPER_DEVBITS(typer);

-			order = get_order((1UL << ids) * entry_size);
-			if (order >= MAX_ORDER) {
-				order = MAX_ORDER - 1;
-				pr_warn("%s: Device Table too large, reduce its page order to %u\n",
-					its->msi_chip.of_node->full_name, order);
+			alloc_size = (1UL << ids) * entry_size;
+			break;
 			}
+
+		default:
+			alloc_size = PAGE_SIZE;
+			break;
+		}
+
+		psz = PAGE_SIZE;
+		shr = GITS_BASER_InnerShareable;
+		cache = GITS_BASER_WaWb;
+
+		/* Test the fields that maybe read-only */
+		tmp = shr | (~val & GITS_BASER_PAGE_SIZE_MASK);
+		writeq_relaxed(tmp, its->base + GITS_BASER + i * 8);
+		tmp = readq_relaxed(its->base + GITS_BASER + i * 8);
+
+		/*
+		 * Shareability didn't stick. Just use
+		 * whatever the read reported, which is likely
+		 * to be the only thing this redistributor
+		 * supports. If that's zero, make it
+		 * non-cacheable as well.
+		 */
+		shr = tmp & GITS_BASER_SHAREABILITY_MASK;
+		if (!shr)
+			cache = GITS_BASER_nC;
+
+		/*
+		 * "Page Size" field is read-only. Should ensure the table
+		 * start address and size align to it.
+		 */
+		if (!((val ^ tmp) & GITS_BASER_PAGE_SIZE_MASK))
+			switch (val & GITS_BASER_PAGE_SIZE_MASK) {
+			case GITS_BASER_PAGE_SIZE_4K:
+				psz = SZ_4K;
+				break;
+
+			case GITS_BASER_PAGE_SIZE_16K:
+				psz = SZ_16K;
+				break;
+
+			default:
+				psz = SZ_64K;
+				break;
+			}
+
+		alloc_size = ALIGN(alloc_size, psz);
+
+		order = get_order(alloc_size);
+		if (order >= MAX_ORDER) {
+			order = MAX_ORDER - 1;
+			pr_warn("%s: Device Table too large, reduce its page order to %u\n",
+				its->msi_chip.of_node->full_name, order);
 		}

 		alloc_size = (1 << order) * PAGE_SIZE;
@@ -845,7 +895,6 @@ static int its_alloc_tables(struct its_node *its)

 		its->tables[i] = base;

-retry_baser:
 		val = (virt_to_phys(base) 				 |
 		       (type << GITS_BASER_TYPE_SHIFT)			 |
 		       ((entry_size - 1) << GITS_BASER_ENTRY_SIZE_SHIFT) |
@@ -870,40 +919,10 @@ retry_baser:
 		writeq_relaxed(val, its->base + GITS_BASER + i * 8);
 		tmp = readq_relaxed(its->base + GITS_BASER + i * 8);

-		if ((val ^ tmp) & GITS_BASER_SHAREABILITY_MASK) {
-			/*
-			 * Shareability didn't stick. Just use
-			 * whatever the read reported, which is likely
-			 * to be the only thing this redistributor
-			 * supports. If that's zero, make it
-			 * non-cacheable as well.
-			 */
-			shr = tmp & GITS_BASER_SHAREABILITY_MASK;
-			if (!shr)
-				cache = GITS_BASER_nC;
-			goto retry_baser;
-		}
-
-		if ((val ^ tmp) & GITS_BASER_PAGE_SIZE_MASK) {
-			/*
-			 * Page size didn't stick. Let's try a smaller
-			 * size and retry. If we reach 4K, then
-			 * something is horribly wrong...
-			 */
-			switch (psz) {
-			case SZ_16K:
-				psz = SZ_4K;
-				goto retry_baser;
-			case SZ_64K:
-				psz = SZ_16K;
-				goto retry_baser;
-			}
-		}
-
 		if (val != tmp) {
 			pr_err("ITS: %s: GITS_BASER%d doesn't stick: %lx %lx\n",
 			       its->msi_chip.of_node->full_name, i,
-			       (unsigned long) val, (unsigned long) tmp);
+			       (unsigned long)val, (unsigned long)tmp);
 			err = -ENXIO;
 			goto out_free;
 		}
--
1.8.0