1 files changed, 50 insertions, 3 deletions

diff --git a/drivers/misc/k3_esm.c b/drivers/misc/k3_esm.c
index cc2a23dd66..41faeb3d85 100644
--- a/drivers/misc/k3_esm.c
+++ b/drivers/misc/k3_esm.c
@@ -16,17 +16,57 @@
 
 #define ESM_SFT_RST			0x0c
 #define ESM_SFT_RST_KEY			0x0f
+#define ESM_EN				0x08
+#define ESM_EN_KEY			0x0f
 
 #define ESM_STS(i)			(0x404 + (i) / 32 * 0x20)
+#define ESM_STS_MASK(i)			(1 << ((i) % 32))
 #define ESM_PIN_EN_SET_OFFSET(i)	(0x414 + (i) / 32 * 0x20)
-#define ESM_PIN_MASK(i)			BIT((i) & 0x1f)
+#define ESM_PIN_MASK(i)			(1 << ((i) % 32))
+#define ESM_INTR_EN_SET_OFFSET(i)	(0x408 + (i) / 32 * 0x20)
+#define ESM_INTR_MASK(i)		(1 << ((i) % 32))
+#define ESM_INTR_PRIO_SET_OFFSET(i)	(0x410 + (i) / 32 * 0x20)
+#define ESM_INTR_PRIO_MASK(i)		(1 << ((i) % 32))
 
 static void esm_pin_enable(void __iomem *base, int pin)
 {
+	u32 value;
+
+	value = readl(base + ESM_PIN_EN_SET_OFFSET(pin));
+	value |= ESM_PIN_MASK(pin);
 	/* Enable event */
-	writel(ESM_PIN_MASK(pin), base + ESM_PIN_EN_SET_OFFSET(pin));
+	writel(value, base + ESM_PIN_EN_SET_OFFSET(pin));
+}
+
+static void esm_intr_enable(void __iomem *base, int pin)
+{
+	u32 value;
+
+	value = readl(base + ESM_INTR_EN_SET_OFFSET(pin));
+	value |= ESM_INTR_MASK(pin);
+	/* Enable Interrupt event */
+	writel(value, base + ESM_INTR_EN_SET_OFFSET(pin));
+}
+
+static void esm_intr_prio_set(void __iomem *base, int pin)
+{
+	u32 value;
+
+	value = readl(base + ESM_INTR_PRIO_SET_OFFSET(pin));
+	value |= ESM_INTR_PRIO_MASK(pin);
+	/* Set to priority */
+	writel(value, base + ESM_INTR_PRIO_SET_OFFSET(pin));
 }
 
+static void esm_clear_raw_status(void __iomem *base, int pin)
+{
+	u32 value;
+
+	value = readl(base + ESM_STS(pin));
+	value |= ESM_STS_MASK(pin);
+	/* Clear Event status */
+	writel(value, base + ESM_STS(pin));
+}
 /**
  * k3_esm_probe: configures ESM based on DT data
  *
@@ -67,8 +107,15 @@ static int k3_esm_probe(struct udevice *dev)
 	/* Clear any pending events */
 	writel(ESM_SFT_RST_KEY, base + ESM_SFT_RST);
 
-	for (i = 0; i < num_pins; i++)
+	for (i = 0; i < num_pins; i++) {
+		esm_intr_prio_set(base, pins[i]);
+		esm_clear_raw_status(base, pins[i]);
 		esm_pin_enable(base, pins[i]);
+		esm_intr_enable(base, pins[i]);
+	}
+
+	/* Enable ESM */
+	writel(ESM_EN_KEY, base + ESM_EN);
 
 free_pins:
 	kfree(pins);