1 files changed, 86 insertions, 0 deletions

diff --git a/src/soc/intel/apollolake/itss.c b/src/soc/intel/apollolake/itss.c
index 60d3e06406..9c49d6c655 100644
--- a/src/soc/intel/apollolake/itss.c
+++ b/src/soc/intel/apollolake/itss.c
@@ -13,6 +13,8 @@
  * GNU General Public License for more details.
  */
 
+#include <commonlib/helpers.h>
+#include <console/console.h>
 #include <stdint.h>
 #include <soc/iosf.h>
 #include <soc/itss.h>
@@ -21,6 +23,7 @@
 #define ITSS_MAX_IRQ		119
 #define IPC0			0x3200
 #define IRQS_PER_IPC		32
+#define NUM_IPC_REGS		((ITSS_MAX_IRQ + IRQS_PER_IPC - 1)/IRQS_PER_IPC)
 
 void itss_set_irq_polarity(int irq, int active_low)
 {
@@ -41,3 +44,86 @@ void itss_set_irq_polarity(int irq, int active_low)
 	val |= active_low ? mask : 0;
 	iosf_write(port, reg, val);
 }
+
+static uint32_t irq_snapshot[NUM_IPC_REGS];
+
+void itss_snapshot_irq_polarities(int start, int end)
+{
+	int i;
+	int reg_start;
+	int reg_end;
+	const uint16_t port = IOSF_ITSS_PORT_ID;
+
+	if (start < 0 || start > ITSS_MAX_IRQ ||
+	    end < 0 || end > ITSS_MAX_IRQ || end < start)
+		return;
+
+	reg_start = start / IRQS_PER_IPC;
+	reg_end = (end + IRQS_PER_IPC - 1) / IRQS_PER_IPC;
+
+	for (i = reg_start; i < reg_end; i++) {
+		uint16_t reg = IPC0 + sizeof(uint32_t) * i;
+		irq_snapshot[i] = iosf_read(port, reg);
+	}
+}
+
+static void show_irq_polarities(const char *msg)
+{
+	int i;
+	const uint16_t port = IOSF_ITSS_PORT_ID;
+
+	printk(BIOS_INFO, "ITSS IRQ Polarities %s:\n", msg);
+	for (i = 0; i < NUM_IPC_REGS; i++) {
+		uint16_t reg = IPC0 + sizeof(uint32_t) * i;
+		printk(BIOS_INFO, "IPC%d: 0x%08x\n", i, iosf_read(port, reg));
+	}
+}
+
+void itss_restore_irq_polarities(int start, int end)
+{
+	int i;
+	int reg_start;
+	int reg_end;
+	const uint16_t port = IOSF_ITSS_PORT_ID;
+
+	if (start < 0 || start > ITSS_MAX_IRQ ||
+	    end < 0 || end > ITSS_MAX_IRQ || end < start)
+		return;
+
+	show_irq_polarities("Before");
+
+	reg_start = start / IRQS_PER_IPC;
+	reg_end = (end + IRQS_PER_IPC - 1) / IRQS_PER_IPC;
+
+	for (i = reg_start; i < reg_end; i++) {
+		uint32_t mask;
+		uint32_t val;
+		uint16_t reg;
+		int irq_start;
+		int irq_end;
+
+		irq_start = i * IRQS_PER_IPC;
+		irq_end = MIN(irq_start + IRQS_PER_IPC - 1, ITSS_MAX_IRQ);
+
+		if (start > irq_end)
+			continue;
+		if (end < irq_start)
+			break;
+
+		/* Track bits within the bounds of of the register. */
+		irq_start = MAX(start, irq_start) % IRQS_PER_IPC;
+		irq_end = MIN(end, irq_end) % IRQS_PER_IPC;
+
+		/* Create bitmask of the inclusive range of start and end. */
+		mask = (((1U << irq_end) - 1) | (1U << irq_end));
+		mask &= ~((1U << irq_start) - 1);
+
+		reg = IPC0 + sizeof(uint32_t) * i;
+		val = iosf_read(port, reg);
+		val &= ~mask;
+		val |= mask & irq_snapshot[i];
+		iosf_write(port, reg, val);
+	}
+
+	show_irq_polarities("After");
+}