---
drivers/firmware/arm_scmi/clock.c | 42 +++++++++++++++++++++++++++++++
include/linux/scmi_protocol.h     |  6 +++++
2 files changed, 48 insertions(+)

diff --git a/drivers/firmware/arm_scmi/clock.c b/drivers/firmware/arm_scmi/clock.c
index ab36871650a1..54e8b59c3941 100644
--- a/drivers/firmware/arm_scmi/clock.c
+++ b/drivers/firmware/arm_scmi/clock.c

@@ -8,6 +8,7 @@

#include <linux/module.h>
#include <linux/limits.h>
#include <linux/sort.h>
+#include <asm/div64.h>

#include "protocols.h"
#include "notify.h"

@@ -624,6 +625,46 @@ static int scmi_clock_rate_set(const struct scmi_protocol_handle *ph,

	return ret;
}

+static int scmi_clock_determine_rate(const struct scmi_protocol_handle *ph,
+				     u32 clk_id, unsigned long *rate)
+{
+	u64 fmin, fmax, ftmp;
+	struct scmi_clock_info *clk;
+	struct clock_info *ci = ph->get_priv(ph);
+
+	if (!rate)
+		return -EINVAL;
+
+	clk = scmi_clock_domain_lookup(ci, clk_id);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	/*
+	 * If we can't figure out what rate it will be, so just return the
+	 * rate back to the caller.
+	 */
+	if (clk->rate_discrete)
+		return 0;
+
+	fmin = clk->range.min_rate;
+	fmax = clk->range.max_rate;
+	if (*rate <= fmin) {
+		*rate = fmin;
+		return 0;
+	} else if (*rate >= fmax) {
+		*rate = fmax;
+		return 0;
+	}
+
+	ftmp = *rate - fmin;
+	ftmp += clk->range.step_size - 1; /* to round up */
+	do_div(ftmp, clk->range.step_size);
+
+	*rate = ftmp * clk->range.step_size + fmin;
+
+	return 0;
+}
+
static int
scmi_clock_config_set(const struct scmi_protocol_handle *ph, u32 clk_id,
		      enum clk_state state,

@@ -936,6 +977,7 @@ static const struct scmi_clk_proto_ops clk_proto_ops = {

	.info_get = scmi_clock_info_get,
	.rate_get = scmi_clock_rate_get,
	.rate_set = scmi_clock_rate_set,
+	.determine_rate = scmi_clock_determine_rate,
	.enable = scmi_clock_enable,
	.disable = scmi_clock_disable,
	.state_get = scmi_clock_state_get,

diff --git a/include/linux/scmi_protocol.h b/include/linux/scmi_protocol.h
index aafaac1496b0..28579c145045 100644
--- a/include/linux/scmi_protocol.h
+++ b/include/linux/scmi_protocol.h

@@ -91,6 +91,10 @@ enum scmi_clock_oem_config {
 * @info_get: get the information of the specified clock
 * @rate_get: request the current clock rate of a clock
 * @rate_set: set the clock rate of a clock
+ * @determine_rate: determine the effective rate that can be supported by a
+ *		    clock calculating the closest allowed rate.
+ *		    Note that @rate is an input/output parameter used both to
+ *		    describe the requested rate and report the closest match
 * @enable: enables the specified clock
 * @disable: disables the specified clock
 * @state_get: get the status of the specified clock

@@ -108,6 +112,8 @@ struct scmi_clk_proto_ops {

			u64 *rate);
	int (*rate_set)(const struct scmi_protocol_handle *ph, u32 clk_id,
			u64 rate);
+	int (*determine_rate)(const struct scmi_protocol_handle *ph, u32 clk_id,
+			      unsigned long *rate);
	int (*enable)(const struct scmi_protocol_handle *ph, u32 clk_id,
		      bool atomic);
	int (*disable)(const struct scmi_protocol_handle *ph, u32 clk_id,
-- 
2.53.0