From edf00d8e5190369d5c703ee056d615221a58ba70 Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Fri, 4 Oct 2024 21:54:58 +0300
Subject: [PATCH] bdk: bpmp: add state set function

Some states are controlled via software. So add a function for that.
---
 bdk/soc/bpmp.c |  8 ++++++++
 bdk/soc/bpmp.h | 11 +++++++++++
 2 files changed, 19 insertions(+)

diff --git a/bdk/soc/bpmp.c b/bdk/soc/bpmp.c
index e563d45..3979c2a 100644
--- a/bdk/soc/bpmp.c
+++ b/bdk/soc/bpmp.c
@@ -237,6 +237,7 @@ void bpmp_clk_rate_relaxed(bool enable)
 // APB clock affects RTC, PWM, MEMFETCH, APE, USB, SOR PWM,
 // I2C host, DC/DSI/DISP. UART gives extra stress.
 // 92: 100% success ratio. 93-94: 595-602MHz has 99% success ratio. 95: 608MHz less.
+// APB clock max is supposed to be 204 MHz though.
 static const u8 pll_divn[] = {
 	0,   // BPMP_CLK_NORMAL:      408MHz  0% - 136MHz APB.
 	85,  // BPMP_CLK_HIGH_BOOST:  544MHz 33% - 136MHz APB.
@@ -300,6 +301,13 @@ void bpmp_clk_rate_set(bpmp_freq_t fid)
 	}
 }
 
+// State is reset to RUN on any clock or source set via SW.
+void bpmp_state_set(bpmp_state_t state)
+{
+	u32 cfg = CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) & ~0xF0000000u;
+	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = cfg | (state << 28u);
+}
+
 // The following functions halt BPMP to reduce power while sleeping.
 // They are not as accurate as RTC at big values but they guarantee time+ delay.
 void bpmp_usleep(u32 us)
diff --git a/bdk/soc/bpmp.h b/bdk/soc/bpmp.h
index 129ea57..ace1290 100644
--- a/bdk/soc/bpmp.h
+++ b/bdk/soc/bpmp.h
@@ -54,6 +54,16 @@ typedef enum
 	BPMP_CLK_MAX
 } bpmp_freq_t;
 
+typedef enum
+{
+	BPMP_STATE_STANDBY = 0, // 32KHz.
+	BPMP_STATE_IDLE    = 1,
+	BPMP_STATE_RUN     = 2,
+
+	BPMP_STATE_IRQ     = BIT(2),
+	BPMP_STATE_FIQ     = BIT(3),
+} bpmp_state_t;
+
 #define BPMP_CLK_LOWEST_BOOST  BPMP_CLK_HIGH2_BOOST
 #define BPMP_CLK_LOWER_BOOST   BPMP_CLK_SUPER_BOOST
 #define BPMP_CLK_DEFAULT_BOOST BPMP_CLK_HYPER_BOOST
@@ -65,6 +75,7 @@ void bpmp_mmu_disable();
 void bpmp_clk_rate_relaxed(bool enable);
 void bpmp_clk_rate_get();
 void bpmp_clk_rate_set(bpmp_freq_t fid);
+void bpmp_state_set(bpmp_state_t state);
 void bpmp_usleep(u32 us);
 void bpmp_msleep(u32 ms);
 void bpmp_halt();