[RFC PATCH 3/3] cpufreq: Add Rust based cpufreq-dt driver

[Viresh Kumar <viresh.kumar@linaro.org>]

This commit adds a Rust based cpufreq-dt driver, which covers most of
the functionality of the existing C based driver. Only a handful of
things are left, like fetching platform data from cpufreq-dt-platdev.c.

This is tested with the help of QEMU for now and switching of
frequencies work as expected.

Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
---
 drivers/cpufreq/Kconfig        |  12 ++
 drivers/cpufreq/Makefile       |   1 +
 drivers/cpufreq/rcpufreq_dt.rs | 264 +++++++++++++++++++++++++++++++++
 3 files changed, 277 insertions(+)
 create mode 100644 drivers/cpufreq/rcpufreq_dt.rs

diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index 94e55c40970a..eb9359bd3c5c 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -217,6 +217,18 @@ config CPUFREQ_DT
 
 	  If in doubt, say N.
 
+config CPUFREQ_DT_RUST
+	tristate "Rust based Generic DT based cpufreq driver"
+	depends on HAVE_CLK && OF && RUST
+	select CPUFREQ_DT_PLATDEV
+	select PM_OPP
+	help
+	  This adds a Rust based generic DT based cpufreq driver for frequency
+	  management.  It supports both uniprocessor (UP) and symmetric
+	  multiprocessor (SMP) systems.
+
+	  If in doubt, say N.
+
 config CPUFREQ_DT_PLATDEV
 	tristate "Generic DT based cpufreq platdev driver"
 	depends on OF
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 8d141c71b016..4981d908b803 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_CPU_FREQ_GOV_COMMON)		+= cpufreq_governor.o
 obj-$(CONFIG_CPU_FREQ_GOV_ATTR_SET)	+= cpufreq_governor_attr_set.o
 
 obj-$(CONFIG_CPUFREQ_DT)		+= cpufreq-dt.o
+obj-$(CONFIG_CPUFREQ_DT_RUST)		+= rcpufreq_dt.o
 obj-$(CONFIG_CPUFREQ_DT_PLATDEV)	+= cpufreq-dt-platdev.o
 
 # Traces
diff --git a/drivers/cpufreq/rcpufreq_dt.rs b/drivers/cpufreq/rcpufreq_dt.rs
new file mode 100644
index 000000000000..d29182eba75e
--- /dev/null
+++ b/drivers/cpufreq/rcpufreq_dt.rs
@@ -0,0 +1,264 @@
+// SPDX-License-Identifier: GPL-2.0
+
+//! Rust based implementation of the cpufreq-dt driver.
+
+use core::{format_args, ptr};
+
+use kernel::{
+    b_str, bindings, c_str, cpufreq, define_of_id_table,
+    device::{self, Device, RawDevice},
+    error::{code::*, to_result},
+    fmt,
+    macros::vtable,
+    module_platform_driver, of, opp, platform,
+    prelude::*,
+    str::CString,
+    sync::Arc,
+};
+
+// Finds exact supply name from the OF node.
+fn find_supply_name_exact(np: *mut bindings::device_node, name: &str) -> Option<CString> {
+    let sname = CString::try_from_fmt(fmt!("{}-supply", name)).ok()?;
+
+    // SAFETY: The OF node is guaranteed by the C code to be valid.
+    let pp = unsafe { bindings::of_find_property(np, sname.as_ptr() as *mut _, ptr::null_mut()) };
+    if pp.is_null() {
+        None
+    } else {
+        CString::try_from_fmt(fmt!("{}", name)).ok()
+    }
+}
+
+// Finds supply name for the CPU from DT.
+fn find_supply_names(dev: &Device, cpu: u32) -> Option<Vec<CString>> {
+    // SAFETY: The requirements are satisfied by the existence of `RawDevice` and its safety
+    // requirements. The OF node is guaranteed by the C code to be valid or NULL and
+    // `of_node_get()` handles both the cases safely.
+    let np = unsafe { bindings::of_node_get((*dev.raw_device()).of_node) };
+    if np.is_null() {
+        return None;
+    }
+
+    // Try "cpu0" for older DTs.
+    let mut name = if cpu == 0 {
+        find_supply_name_exact(np, "cpu0")
+    } else {
+        None
+    };
+
+    if name.is_none() {
+        name = find_supply_name_exact(np, "cpu");
+    }
+
+    // SAFETY: It is safe to drop reference to the OF node we just took.
+    unsafe { bindings::of_node_put(np) };
+
+    match name {
+        Some(name) => {
+            let mut list = Vec::try_with_capacity(1).ok()?;
+            list.try_push(name).ok()?;
+
+            // SAFETY: The slice is fully initialized now.
+            Some(list)
+        }
+
+        None => None,
+    }
+}
+
+// Represents the cpufreq dt device.
+struct CPUFreqDTDevice {
+    opp_table: opp::Table,
+    freq_table: opp::FreqTable,
+    #[allow(dead_code)]
+    config: Option<opp::Config<Self>>,
+    #[allow(dead_code)]
+    clk: cpufreq::Clk,
+}
+
+#[vtable]
+impl opp::ConfigOps for CPUFreqDTDevice {}
+
+#[vtable]
+impl cpufreq::DriverOps for CPUFreqDTDevice {
+    type PData = Arc<Self>;
+
+    fn init(policy: &mut cpufreq::Policy) -> Result<Self::PData> {
+        let cpu = policy.cpu();
+
+        // SAFETY: It is safe to call `get_cpu_device()` for any CPU.
+        let dev = unsafe { bindings::get_cpu_device(cpu) };
+        if dev.is_null() {
+            return Err(ENODEV);
+        }
+
+        // SAFETY: `dev` is valid, non-null, and has a non-zero reference count.
+        let dev = unsafe { Device::new(dev) };
+
+        policy.set_cpus(cpu);
+
+        let config = if let Some(names) = find_supply_names(&dev, cpu) {
+            let mut config = opp::Config::<Self>::new();
+            config.set_regulator_names(names)?;
+            config.set(&dev)?;
+            Some(config)
+        } else {
+            None
+        };
+
+        // Get OPP-sharing information from "operating-points-v2" bindings.
+        let fallback = match opp::Table::of_sharing_cpus(&dev, policy.cpus()) {
+            Ok(_) => false,
+            Err(e) => {
+                if e != ENOENT {
+                    return Err(e);
+                }
+
+                // "operating-points-v2" not supported. If the platform hasn't
+                // set sharing CPUs, fallback to all CPUs share the `Policy`
+                // for backward compatibility.
+                opp::Table::sharing_cpus(&dev, policy.cpus()).is_err()
+            }
+        };
+
+        // Initialize OPP tables for all policy cpus.
+        //
+        // For platforms not using "operating-points-v2" bindings, we do this
+        // before updating policy cpus. Otherwise, we will end up creating
+        // duplicate OPPs for the CPUs.
+        //
+        // OPPs might be populated at runtime, don't fail for error here unless
+        // it is -EPROBE_DEFER.
+        let mut opp_table = match opp::Table::from_of_cpumask(&dev, policy.cpus()) {
+            Ok(table) => table,
+            Err(e) => {
+                if e == EPROBE_DEFER {
+                    return Err(e);
+                }
+
+                // The table is added dynamically ?
+                opp::Table::from_dev(&dev)?
+            }
+        };
+
+        // The OPP table must be initialized, statically or dynamically, by this point.
+        opp_table.opp_count()?;
+
+        // Set sharing cpus for fallback scenario.
+        if fallback {
+            policy.set_all_cpus();
+            opp_table.set_sharing_cpus(policy.cpus())?;
+        }
+
+        let mut transition_latency = opp_table.max_transition_latency() as u32;
+        if transition_latency == 0 {
+            transition_latency = cpufreq::ETERNAL_LATENCY;
+        }
+
+        let freq_table = opp_table.to_cpufreq_table()?;
+        policy.set_freq_table(freq_table.table());
+        policy.set_dvfs_possible_from_any_cpu();
+        policy.set_suspend_freq((opp_table.suspend_freq() / 1000) as u32);
+        policy.set_transition_latency(transition_latency);
+        let clk = policy.set_clk(&dev, None)?;
+
+        Ok(Arc::try_new(CPUFreqDTDevice {
+            opp_table,
+            config,
+            freq_table,
+            clk,
+        })?)
+    }
+
+    fn exit(_policy: &mut cpufreq::Policy, _data: Option<Self::PData>) -> Result<()> {
+        Ok(())
+    }
+
+    fn online(_policy: &mut cpufreq::Policy) -> Result<()> {
+        // We did light-weight tear down earlier, nothing to do here.
+        Ok(())
+    }
+
+    fn offline(_policy: &mut cpufreq::Policy) -> Result<()> {
+        // Preserve policy->data and don't free resources on light-weight
+        // tear down.
+        Ok(())
+    }
+
+    fn suspend(policy: &mut cpufreq::Policy) -> Result<()> {
+        // SAFETY: By the type invariants, we know that `self` owns a reference, so it is safe to
+        // use it now.
+        to_result(unsafe { bindings::cpufreq_generic_suspend(policy.as_ptr()) })
+    }
+
+    fn verify(data: &mut cpufreq::PolicyData) -> Result<()> {
+        // SAFETY: By the type invariants, we know that `data` owns a reference received from the C
+        // code, so it is safe to use it now.
+        to_result(unsafe { bindings::cpufreq_generic_frequency_table_verify(data.as_ptr()) })
+    }
+
+    fn target_index(policy: &mut cpufreq::Policy, index: u32) -> Result<()> {
+        let data = match policy.data::<Self::PData>() {
+            Some(data) => data,
+            None => return Err(ENOENT),
+        };
+
+        let freq = data.freq_table.freq(index.try_into().unwrap())? as u64;
+        data.opp_table.set_rate(freq * 1000)
+    }
+
+    fn get(policy: &mut cpufreq::Policy) -> Result<u32> {
+        // SAFETY: It is safe to call `cpufreq_generic_get()` for policy's CPU.
+        Ok(unsafe { bindings::cpufreq_generic_get(policy.cpu()) })
+    }
+
+    fn set_boost(_policy: &mut cpufreq::Policy, _state: i32) -> Result<()> {
+        Ok(())
+    }
+
+    fn register_em(policy: &mut cpufreq::Policy) {
+        // SAFETY: By the type invariants, we know that `self` owns a reference, so it is safe to
+        // use it now.
+        unsafe { bindings::cpufreq_register_em_with_opp(policy.as_ptr()) };
+    }
+}
+
+type DeviceData = device::Data<cpufreq::Registration<CPUFreqDTDevice>, (), ()>;
+
+struct CPUFreqDTDriver;
+
+impl platform::Driver for CPUFreqDTDriver {
+    type Data = Arc<DeviceData>;
+
+    define_of_id_table! {(), [
+        (of::DeviceId::Compatible(b_str!("operating-points-v2")), None),
+    ]}
+
+    fn probe(_dev: &mut platform::Device, _id_info: Option<&Self::IdInfo>) -> Result<Self::Data> {
+        let data = Arc::<DeviceData>::from(kernel::new_device_data!(
+            cpufreq::Registration::new(),
+            (),
+            (),
+            "CPUFreqDT::Registration"
+        )?);
+        let flags = cpufreq::flags::NEED_INITIAL_FREQ_CHECK | cpufreq::flags::IS_COOLING_DEV;
+        let boost = true;
+
+        data.registrations()
+            .ok_or(ENXIO)?
+            .as_pinned_mut()
+            .register(c_str!("cpufreq-dt"), (), flags, boost)?;
+
+        pr_info!("CPUFreq DT driver registered\n");
+
+        Ok(data)
+    }
+}
+
+module_platform_driver! {
+    type: CPUFreqDTDriver,
+    name: "cpufreq_dt",
+    author: "Viresh Kumar <viresh.kumar@linaro.org>",
+    description: "Generic CPUFreq DT driver",
+    license: "GPL v2",
+}
-- 
2.31.1.272.g89b43f80a514