3 files changed, 43 insertions, 11 deletions

diff --git a/arch/x86/cpu/apollolake/fsp_s.c b/arch/x86/cpu/apollolake/fsp_s.c
index 3a54297a28d..e54b0ac1047 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 23a4d633d2d..9ef797b41b6 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>
@@ -269,9 +270,11 @@ int cpu_init_r(void)
 #ifndef CONFIG_EFI_STUB
 int reserve_arch(void)
 {
-#ifdef CONFIG_ENABLE_MRC_CACHE
-	mrccache_reserve();
-#endif
+	struct udevice *itss;
+	int ret;
+
+	if (IS_ENABLED(CONFIG_ENABLE_MRC_CACHE))
+		mrccache_reserve();
 
 #ifdef CONFIG_SEABIOS
 	high_table_reserve();
@@ -288,6 +291,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 963afa8f5be..fe84ebe29f7 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]);
 	}