[PATCH v6 2/4] soc: samsung: usi: add a routine for unconfiguring the ip

[Ivaylo Ivanov <ivo.ivanov.ivanov1@xxxxxxxxx>]

Add a devm_add_action_or_reset() routine for unconfiguring the USI IP
block whenever the device gets removed.

Signed-off-by: Ivaylo Ivanov <ivo.ivanov.ivanov1@xxxxxxxxx>
---
 drivers/soc/samsung/exynos-usi.c | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/drivers/soc/samsung/exynos-usi.c b/drivers/soc/samsung/exynos-usi.c
index 114352695..85329d4e8 100644
--- a/drivers/soc/samsung/exynos-usi.c
+++ b/drivers/soc/samsung/exynos-usi.c
@@ -174,6 +174,30 @@ static int exynos_usi_configure(struct exynos_usi *usi)
 	return 0;
 }
 
+static void exynos_usi_unconfigure(void *data)
+{
+	struct exynos_usi *usi = data;
+	u32 val;
+	int ret;
+
+	ret = clk_bulk_prepare_enable(usi->data->num_clks, usi->clks);
+	if (ret)
+		return;
+
+	/* Make sure that we've stopped providing the clock to USI IP */
+	val = readl(usi->regs + USI_OPTION);
+	val &= ~USI_OPTION_CLKREQ_ON;
+	val |= ~USI_OPTION_CLKSTOP_ON;
+	writel(val, usi->regs + USI_OPTION);
+
+	/* Set USI block state to reset */
+	val = readl(usi->regs + USI_CON);
+	val |= USI_CON_RESET;
+	writel(val, usi->regs + USI_CON);
+
+	clk_bulk_disable_unprepare(usi->data->num_clks, usi->clks);
+}
+
 static int exynos_usi_parse_dt(struct device_node *np, struct exynos_usi *usi)
 {
 	int ret;
@@ -255,6 +279,10 @@ static int exynos_usi_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
+	ret = devm_add_action_or_reset(&pdev->dev, exynos_usi_unconfigure, usi);
+	if (ret)
+		return ret;
+
 	/* Make it possible to embed protocol nodes into USI np */
 	return of_platform_populate(np, NULL, NULL, dev);
 }
-- 
2.43.0