From 44c787e88b5e0ea96920a1f01b49f9bc67b5dc48 Mon Sep 17 00:00:00 2001
From: Nico Weber <thakis@chromium.org>
Date: Sat, 25 Sep 2021 23:28:50 -0400
Subject: [PATCH] Kernel: Add Mailbox::set_clock_rate()

---
 Kernel/Prekernel/Arch/aarch64/Mailbox.cpp | 20 ++++++++++++++++++++
 Kernel/Prekernel/Arch/aarch64/Mailbox.h   | 19 +++++++++++++++++++
 2 files changed, 39 insertions(+)

diff --git a/Kernel/Prekernel/Arch/aarch64/Mailbox.cpp b/Kernel/Prekernel/Arch/aarch64/Mailbox.cpp
index 28779e92d9..005869c68b 100644
--- a/Kernel/Prekernel/Arch/aarch64/Mailbox.cpp
+++ b/Kernel/Prekernel/Arch/aarch64/Mailbox.cpp
@@ -75,6 +75,7 @@ bool Mailbox::call(u8 channel, u32 volatile* __attribute__((aligned(16))) messag
 }
 
 constexpr u32 MBOX_TAG_GET_FIRMWARE_VERSION = 0x0000'0001;
+constexpr u32 MBOX_TAG_SET_CLOCK_RATE = 0x0003'8002;
 
 u32 Mailbox::query_firmware_version()
 {
@@ -96,4 +97,23 @@ u32 Mailbox::query_firmware_version()
     return 0xffff'ffff;
 }
 
+u32 Mailbox::set_clock_rate(ClockID clock_id, u32 rate_hz, bool skip_setting_turbo)
+{
+    u32 __attribute__((aligned(16))) message[9];
+    message[0] = sizeof(message);
+    message[1] = MBOX_REQUEST;
+
+    message[2] = MBOX_TAG_SET_CLOCK_RATE;
+    message[3] = 12; // Tag data size.
+    message[4] = MBOX_REQUEST;
+    message[5] = static_cast<u32>(clock_id);
+    message[6] = rate_hz;
+    message[7] = skip_setting_turbo ? 1 : 0;
+
+    message[8] = 0;
+
+    call(ARM_TO_VIDEOCORE_CHANNEL, message);
+    return message[6];
+}
+
 }
diff --git a/Kernel/Prekernel/Arch/aarch64/Mailbox.h b/Kernel/Prekernel/Arch/aarch64/Mailbox.h
index 006812d023..9f06cbea8c 100644
--- a/Kernel/Prekernel/Arch/aarch64/Mailbox.h
+++ b/Kernel/Prekernel/Arch/aarch64/Mailbox.h
@@ -17,6 +17,25 @@ public:
     static bool call(u8 channel, u32 volatile* __attribute__((aligned(16))) data);
 
     static u32 query_firmware_version();
+
+    enum class ClockID {
+        Reserved = 0,
+        EMMC = 1,
+        UART = 2,
+        ARM = 3,
+        CORE = 4,
+        V3D = 5,
+        H264 = 6,
+        ISP = 7,
+        SDRAM = 8,
+        PIXEL = 9,
+        PWM = 10,
+        HEVC = 11,
+        EMMC2 = 12,
+        M2MC = 13,
+        PIXEL_BVB = 14,
+    };
+    static u32 set_clock_rate(ClockID, u32 rate_hz, bool skip_setting_turbo = true);
 };
 
 }