From f452d916c9c69b561b40c1ed807553f726e973de Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Mon, 9 May 2022 06:08:39 +0300
Subject: [PATCH] bdk: clock: add ext peripheral clock control

---
 bdk/soc/clock.c | 35 +++++++++++++++++++++++++++++++++++
 bdk/soc/clock.h |  6 ++++++
 bdk/soc/pmc.h   |  8 ++++++++
 3 files changed, 49 insertions(+)

diff --git a/bdk/soc/clock.c b/bdk/soc/clock.c
index eda6d0b..e9e4fb5 100644
--- a/bdk/soc/clock.c
+++ b/bdk/soc/clock.c
@@ -17,6 +17,7 @@
 
 #include <soc/clock.h>
 #include <soc/hw_init.h>
+#include <soc/pmc.h>
 #include <soc/t210.h>
 #include <storage/sdmmc.h>
 #include <utils/util.h>
@@ -109,6 +110,12 @@ static clock_t _clock_ahbdma = {
 static clock_t _clock_actmon = {
 	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_ACTMON,  CLK_V_ACTMON,  6, 0 // 19.2MHz.
 };
+static clock_t _clock_extperiph1 = {
+	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH1,  CLK_V_EXTPERIPH1,  0, 0
+};
+static clock_t _clock_extperiph2 = {
+	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH2,  CLK_V_EXTPERIPH2,  2, 202 // 4.0MHz
+};
 
 void clock_enable(const clock_t *clk)
 {
@@ -341,6 +348,34 @@ void clock_disable_actmon()
 	clock_disable(&_clock_actmon);
 }
 
+void clock_enable_extperiph1()
+{
+	clock_enable(&_clock_extperiph1);
+	
+	PMC(APBDEV_PMC_CLK_OUT_CNTRL) |= PMC_CLK_OUT_CNTRL_CLK1_SRC_SEL(OSC_CAR) | PMC_CLK_OUT_CNTRL_CLK1_FORCE_EN;
+	usleep(5);
+}
+
+void clock_disable_extperiph1()
+{
+	PMC(APBDEV_PMC_CLK_OUT_CNTRL) &= ~((PMC_CLK_OUT_CNTRL_CLK1_SRC_SEL(OSC_CAR)) | PMC_CLK_OUT_CNTRL_CLK1_FORCE_EN);
+	clock_disable(&_clock_extperiph1);
+}
+
+void clock_enable_extperiph2()
+{
+	clock_enable(&_clock_extperiph2);
+
+	PMC(APBDEV_PMC_CLK_OUT_CNTRL) |= PMC_CLK_OUT_CNTRL_CLK2_SRC_SEL(OSC_CAR) | PMC_CLK_OUT_CNTRL_CLK2_FORCE_EN;
+	usleep(5);
+}
+
+void clock_disable_extperiph2()
+{
+	PMC(APBDEV_PMC_CLK_OUT_CNTRL) &= ~((PMC_CLK_OUT_CNTRL_CLK2_SRC_SEL(OSC_CAR)) | PMC_CLK_OUT_CNTRL_CLK2_FORCE_EN);
+	clock_disable(&_clock_extperiph2);
+}
+
 void clock_enable_plld(u32 divp, u32 divn, bool lowpower, bool tegra_t210)
 {
 	u32 plld_div = (divp << 20) | (divn << 11) | 1;
diff --git a/bdk/soc/clock.h b/bdk/soc/clock.h
index b14780c..30d4a46 100644
--- a/bdk/soc/clock.h
+++ b/bdk/soc/clock.h
@@ -119,6 +119,7 @@
 #define CLK_RST_CONTROLLER_CLK_SOURCE_I2C4 0x3C4
 #define CLK_RST_CONTROLLER_CLK_SOURCE_ACTMON 0x3E8
 #define CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH1 0x3EC
+#define CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH2 0x3F0
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SYS 0x400
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SOR1 0x410
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SE 0x42C
@@ -670,6 +671,10 @@ void clock_enable_ahbdma();
 void clock_disable_ahbdma();
 void clock_enable_actmon();
 void clock_disable_actmon();
+void clock_enable_extperiph1();
+void clock_disable_extperiph1();
+void clock_enable_extperiph2();
+void clock_disable_extperiph2();
 
 void clock_enable_plld(u32 divp, u32 divn, bool lowpower, bool tegra_t210);
 void clock_enable_pllx();
@@ -678,6 +683,7 @@ void clock_disable_pllc();
 void clock_enable_pllu();
 void clock_disable_pllu();
 void clock_enable_utmipll();
+
 void clock_sdmmc_config_clock_source(u32 *pclock, u32 id, u32 val);
 void clock_sdmmc_get_card_clock_div(u32 *pclock, u16 *pdivisor, u32 type);
 int  clock_sdmmc_is_not_reset_and_enabled(u32 id);
diff --git a/bdk/soc/pmc.h b/bdk/soc/pmc.h
index 50f174c..04182ac 100644
--- a/bdk/soc/pmc.h
+++ b/bdk/soc/pmc.h
@@ -77,6 +77,14 @@
 #define APBDEV_PMC_CLK_OUT_CNTRL     0x1A8
 #define  PMC_CLK_OUT_CNTRL_CLK1_FORCE_EN   BIT(2)
 #define  PMC_CLK_OUT_CNTRL_CLK2_FORCE_EN   BIT(10)
+#define  PMC_CLK_OUT_CNTRL_CLK3_FORCE_EN   BIT(18)
+#define  PMC_CLK_OUT_CNTRL_CLK1_SRC_SEL(src) (((src) & 3) << 6)
+#define  PMC_CLK_OUT_CNTRL_CLK2_SRC_SEL(src) (((src) & 3) << 14)
+#define  PMC_CLK_OUT_CNTRL_CLK3_SRC_SEL(src) (((src) & 3) << 22)
+#define   OSC_DIV1                         0
+#define   OSC_DIV2                         1
+#define   OSC_DIV4                         2
+#define   OSC_CAR                          3
 #define APBDEV_PMC_RST_STATUS        0x1B4
 #define  PMC_RST_STATUS_MASK               7
 #define  PMC_RST_STATUS_POR                0