[v2,34/44] x86: apl: Fix save/restore of ITSS priorities

diff --git a/arch/x86/cpu/apollolake/fsp_s.c b/arch/x86/cpu/apollolake/fsp_s.c
index 3a54297a28..e54b0ac104 100644
--- a/arch/x86/cpu/apollolake/fsp_s.c
+++ b/arch/x86/cpu/apollolake/fsp_s.c
@@ -160,11 +160,6 @@  int arch_fsps_preinit(void)
 	ret = irq_first_device_type(X86_IRQT_ITSS, &itss);
 	if (ret)
 		return log_msg_ret("no itss", ret);
-	/*
-	 * Snapshot the current GPIO IRQ polarities. FSP is setting a default
-	 * policy that doesn't honour boards' requirements
-	 */
-	irq_snapshot_polarities(itss);
 
 	/*
 	 * Clear the GPI interrupt status and enable registers. These
@@ -203,7 +198,11 @@  int arch_fsp_init_r(void)
 	ret = irq_first_device_type(X86_IRQT_ITSS, &itss);
 	if (ret)
 		return log_msg_ret("no itss", ret);
-	/* Restore GPIO IRQ polarities back to previous settings */
+
+	/*
+	 * Restore GPIO IRQ polarities back to previous settings. This was
+	 * stored in reserve_arch() - see X86_IRQT_ITSS
+	 */
 	irq_restore_polarities(itss);
 
 	/* soc_init() */
diff --git a/arch/x86/cpu/cpu.c b/arch/x86/cpu/cpu.c
index baa7dae172..9bc243ebc8 100644
--- a/arch/x86/cpu/cpu.c
+++ b/arch/x86/cpu/cpu.c
@@ -25,6 +25,7 @@ 
 #include <dm.h>
 #include <errno.h>
 #include <init.h>
+#include <irq.h>
 #include <log.h>
 #include <malloc.h>
 #include <syscon.h>
@@ -274,6 +275,9 @@  int cpu_init_r(void)
 #ifndef CONFIG_EFI_STUB
 int reserve_arch(void)
 {
+	struct udevice *itss;
+	int ret;
+
 	if (IS_ENABLED(CONFIG_ENABLE_MRC_CACHE))
 		mrccache_reserve();
 
@@ -291,6 +295,15 @@  int reserve_arch(void)
 			fsp_save_s3_stack();
 		}
 	}
+	ret = irq_first_device_type(X86_IRQT_ITSS, &itss);
+	if (!ret) {
+		/*
+		 * Snapshot the current GPIO IRQ polarities. FSP-S is about to
+		 * run and will set a default policy that doesn't honour boards'
+		 * requirements
+		 */
+		irq_snapshot_polarities(itss);
+	}
 
 	return 0;
 }
diff --git a/arch/x86/cpu/intel_common/itss.c b/arch/x86/cpu/intel_common/itss.c
index 963afa8f5b..fe84ebe29f 100644
--- a/arch/x86/cpu/intel_common/itss.c
+++ b/arch/x86/cpu/intel_common/itss.c
@@ -65,14 +65,23 @@  static int snapshot_polarities(struct udevice *dev)
 	int i;
 
 	reg_start = start / IRQS_PER_IPC;
-	reg_end = (end + IRQS_PER_IPC - 1) / IRQS_PER_IPC;
+	reg_end = DIV_ROUND_UP(end, IRQS_PER_IPC);
 
+	log_info("ITSS IRQ Polarities snapshot %p\n", priv->irq_snapshot);
 	for (i = reg_start; i < reg_end; i++) {
 		uint reg = PCR_ITSS_IPC0_CONF + sizeof(u32) * i;
 
 		priv->irq_snapshot[i] = pcr_read32(dev, reg);
+		log_debug("   - %d, reg %x: irq_snapshot[i] %x\n", i, reg,
+			  priv->irq_snapshot[i]);
 	}
 
+	/* Save the snapshot for use after relocation */
+	gd->start_addr_sp -= sizeof(*priv);
+	gd->start_addr_sp &= ~0xf;
+	gd->arch.itss_priv = (void *)gd->start_addr_sp;
+	memcpy(gd->arch.itss_priv, priv, sizeof(*priv));
+
 	return 0;
 }
 
@@ -91,16 +100,26 @@  static void show_polarities(struct udevice *dev, const char *msg)
 static int restore_polarities(struct udevice *dev)
 {
 	struct itss_priv *priv = dev_get_priv(dev);
+	struct itss_priv *old_priv;
 	const int start = GPIO_IRQ_START;
 	const int end = GPIO_IRQ_END;
 	int reg_start;
 	int reg_end;
 	int i;
 
+	/* Get the snapshot which was stored by the pre-reloc device */
+	old_priv = gd->arch.itss_priv;
+	if (!old_priv)
+		return log_msg_ret("priv", -EFAULT);
+	memcpy(priv->irq_snapshot, old_priv->irq_snapshot,
+	       sizeof(priv->irq_snapshot));
+
 	show_polarities(dev, "Before");
+	log_info("priv->irq_snapshot %p\n", priv->irq_snapshot);
 
 	reg_start = start / IRQS_PER_IPC;
-	reg_end = (end + IRQS_PER_IPC - 1) / IRQS_PER_IPC;
+	reg_end = DIV_ROUND_UP(end, IRQS_PER_IPC);
+
 
 	for (i = reg_start; i < reg_end; i++) {
 		u32 mask;
@@ -125,6 +144,8 @@  static int restore_polarities(struct udevice *dev)
 		mask &= ~((1U << irq_start) - 1);
 
 		reg = PCR_ITSS_IPC0_CONF + sizeof(u32) * i;
+		log_debug("   - %d, reg %x: mask %x, irq_snapshot[i] %x\n",
+			  i, reg, mask, priv->irq_snapshot[i]);
 		pcr_clrsetbits32(dev, reg, mask, mask & priv->irq_snapshot[i]);
 	}
 
diff --git a/arch/x86/include/asm/global_data.h b/arch/x86/include/asm/global_data.h
index 0e64c8a46d..5bc251c0dd 100644
--- a/arch/x86/include/asm/global_data.h
+++ b/arch/x86/include/asm/global_data.h
@@ -121,6 +121,7 @@  struct arch_global_data {
 #ifdef CONFIG_FSP_VERSION2
 	struct fsp_header *fsp_s_hdr;	/* Pointer to FSP-S header */
 #endif
+	void *itss_priv;		/* Private ITSS data pointer */
 	ulong acpi_start;		/* Start address of ACPI tables */
 };
 
diff --git a/arch/x86/include/asm/itss.h b/arch/x86/include/asm/itss.h
index c75d8fe8c2..f7d3240384 100644
--- a/arch/x86/include/asm/itss.h
+++ b/arch/x86/include/asm/itss.h
@@ -16,7 +16,7 @@ 
 
 #define ITSS_MAX_IRQ	119
 #define IRQS_PER_IPC	32
-#define NUM_IPC_REGS	((ITSS_MAX_IRQ + IRQS_PER_IPC - 1) / IRQS_PER_IPC)
+#define NUM_IPC_REGS	DIV_ROUND_UP(ITSS_MAX_IRQ, IRQS_PER_IPC)
 
 /* Max PXRC registers in ITSS */
 #define MAX_PXRC_CONFIG	(PCR_ITSS_PIRQH_ROUT - PCR_ITSS_PIRQA_ROUT + 1)
diff --git a/drivers/misc/irq-uclass.c b/drivers/misc/irq-uclass.c
index 8727a33dd9..94fa233f19 100644
--- a/drivers/misc/irq-uclass.c
+++ b/drivers/misc/irq-uclass.c
@@ -168,7 +168,7 @@  int irq_first_device_type(enum irq_dev_t type, struct udevice **devp)
 
 	ret = uclass_first_device_drvdata(UCLASS_IRQ, type, devp);
 	if (ret)
-		return log_msg_ret("find", ret);
+		return ret;
 
 	return 0;
 }