2 files changed, 67 insertions, 2 deletions

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/cpu/intel_common/p2sb.c b/arch/x86/cpu/intel_common/p2sb.c
index ec35d04ae5..361d4c90cb 100644
--- a/arch/x86/cpu/intel_common/p2sb.c
+++ b/arch/x86/cpu/intel_common/p2sb.c
@@ -16,6 +16,9 @@
 #include <asm/pci.h>
 #include <linux/bitops.h>
 
+#define PCH_P2SB_E0		0xe0
+#define HIDE_BIT		BIT(0)
+
 struct p2sb_platdata {
 #if CONFIG_IS_ENABLED(OF_PLATDATA)
 	struct dtd_intel_p2sb dtplat;
@@ -127,6 +130,40 @@ static int p2sb_probe(struct udevice *dev)
 	return 0;
 }
 
+static void p2sb_set_hide_bit(struct udevice *dev, bool hide)
+{
+	dm_pci_clrset_config8(dev, PCH_P2SB_E0 + 1, HIDE_BIT,
+			      hide ? HIDE_BIT : 0);
+}
+
+static int intel_p2sb_set_hide(struct udevice *dev, bool hide)
+{
+	u16 vendor;
+
+	if (!CONFIG_IS_ENABLED(PCI))
+		return -EPERM;
+	p2sb_set_hide_bit(dev, hide);
+
+	dm_pci_read_config16(dev, PCI_VENDOR_ID, &vendor);
+	if (hide && vendor != 0xffff)
+		return log_msg_ret("hide", -EEXIST);
+	else if (!hide && vendor != PCI_VENDOR_ID_INTEL)
+		return log_msg_ret("unhide", -ENOMEDIUM);
+
+	return 0;
+}
+
+static int p2sb_remove(struct udevice *dev)
+{
+	int ret;
+
+	ret = intel_p2sb_set_hide(dev, true);
+	if (ret)
+		return log_msg_ret("hide", ret);
+
+	return 0;
+}
+
 static int p2sb_child_post_bind(struct udevice *dev)
 {
 #if !CONFIG_IS_ENABLED(OF_PLATDATA)
@@ -143,6 +180,10 @@ static int p2sb_child_post_bind(struct udevice *dev)
 	return 0;
 }
 
+struct p2sb_ops p2sb_ops = {
+	.set_hide	= intel_p2sb_set_hide,
+};
+
 static const struct udevice_id p2sb_ids[] = {
 	{ .compatible = "intel,p2sb" },
 	{ }
@@ -153,9 +194,12 @@ U_BOOT_DRIVER(p2sb_drv) = {
 	.id		= UCLASS_P2SB,
 	.of_match	= p2sb_ids,
 	.probe		= p2sb_probe,
+	.remove		= p2sb_remove,
+	.ops		= &p2sb_ops,
 	.ofdata_to_platdata = p2sb_ofdata_to_platdata,
 	.platdata_auto_alloc_size = sizeof(struct p2sb_platdata),
 	.per_child_platdata_auto_alloc_size =
 		sizeof(struct p2sb_child_platdata),
 	.child_post_bind = p2sb_child_post_bind,
+	.flags		= DM_FLAG_OS_PREPARE,
 };