[PATCH v2 3/3] cpufreq: CPPC: Preserve OSPM-set registers across hotplug and unload

[Sumit Gupta <sumitg@nvidia.com>]

Values written to OSPM-set CPPC registers (via sysfs or the autonomous
boot parameter) can be lost in two ways:

  - Across CPU hotplug: the platform may reset a CPU's registers while it
    is offline.
  - On driver unload: the value the driver wrote is left in the register
    instead of returning to its pre-driver state.

Add a small table-driven mechanism that handles both:

  - On init(), capture each register's firmware value before the
    driver programs anything.
  - On offline(), read back each register's current value (whatever was
    last set via sysfs or the boot parameter) so it can be reapplied, then
    restore the firmware value.
  - On online(), reapply the value captured at offline().

Cover the Autonomous Selection (auto_sel), Energy Performance Preference
(EPP) and Autonomous Activity Window (auto_act_window) registers.

Suggested-by: Pierre Gondois <pierre.gondois@arm.com>
Link: https://lore.kernel.org/all/86780f97-29ee-4a72-b311-38c89434b707@arm.com/
Signed-off-by: Sumit Gupta <sumitg@nvidia.com>
---
 drivers/cpufreq/cppc_cpufreq.c | 130 +++++++++++++++++++++++++++++++++
 1 file changed, 130 insertions(+)

diff --git a/drivers/cpufreq/cppc_cpufreq.c b/drivers/cpufreq/cppc_cpufreq.c
index 432c6a6288a7..9c88512d635c 100644
--- a/drivers/cpufreq/cppc_cpufreq.c
+++ b/drivers/cpufreq/cppc_cpufreq.c
@@ -28,6 +28,123 @@
 
 static struct cpufreq_driver cppc_cpufreq_driver;
 
+/*
+ * OSPM-set CPPC registers tracked for save/restore. A value set via sysfs or
+ * the autonomous boot parameter is reapplied from online() across CPU
+ * hotplug, and the firmware value is restored from offline().
+ */
+enum cppc_saved_reg_id {
+	CPPC_SAVED_AUTO_SEL,
+	CPPC_SAVED_EPP,
+	CPPC_SAVED_AUTO_ACT_WINDOW,
+	CPPC_NR_SAVED_REGS,
+};
+
+struct cppc_saved_reg {
+	int (*get)(int cpu, u64 *val);
+	int (*set)(int cpu, u64 val);
+};
+
+static const struct cppc_saved_reg cppc_saved_regs[CPPC_NR_SAVED_REGS] = {
+	[CPPC_SAVED_AUTO_SEL] = {
+		cppc_get_auto_sel_u64, cppc_set_auto_sel_u64,
+	},
+	[CPPC_SAVED_EPP] = {
+		cppc_get_epp_perf, cppc_set_epp,
+	},
+	[CPPC_SAVED_AUTO_ACT_WINDOW] = {
+		cppc_get_auto_act_window, cppc_set_auto_act_window,
+	},
+};
+
+/*
+ * Per-policy saved state for each register in cppc_saved_regs[]:
+ *   firmware_val  - value before the driver touched it, captured at init()
+ *                   and restored while the policy is offline. U64_MAX if it
+ *                   could not be read
+ *   requested_val - value in effect when the policy last went offline,
+ *                   reapplied at online(). U64_MAX if none
+ */
+struct cppc_saved_state {
+	u64 firmware_val;
+	u64 requested_val;
+};
+
+static DEFINE_PER_CPU(struct cppc_saved_state[CPPC_NR_SAVED_REGS], cppc_saved_state);
+
+/*
+ * Return this policy's saved state. Each policy keeps a single copy, stored in
+ * the per-CPU variable of the first CPU it manages. related_cpus (the policy's
+ * full set of CPUs) never changes while it exists, so this CPU (unlike
+ * policy->cpu) stays the same across CPU hotplug, and every callback reaches
+ * the same copy.
+ */
+static struct cppc_saved_state *cppc_cpufreq_policy_saved_state(struct cpufreq_policy *policy)
+{
+	const struct cpumask *policy_cpus = policy->related_cpus;
+
+	/*
+	 * related_cpus is empty until the core fills it in after init(). Until
+	 * then, fall back to policy->cpus, which has the same first CPU.
+	 */
+	if (cpumask_empty(policy_cpus))
+		policy_cpus = policy->cpus;
+
+	return per_cpu(cppc_saved_state, cpumask_first(policy_cpus));
+}
+
+/*
+ * Capture each register's firmware value before the driver programs anything.
+ */
+static void cppc_cpufreq_save_firmware_regs(struct cpufreq_policy *policy)
+{
+	struct cppc_saved_state *st = cppc_cpufreq_policy_saved_state(policy);
+	unsigned int cpu = policy->cpu;
+	u64 val;
+	int i;
+
+	for (i = 0; i < CPPC_NR_SAVED_REGS; i++) {
+		if (cppc_saved_regs[i].get(cpu, &val))
+			val = U64_MAX;
+		st[i].firmware_val = val;
+		st[i].requested_val = U64_MAX;
+	}
+}
+
+/*
+ * Save each register's current value so online() can later reapply it, then
+ * restore the firmware value to leave the platform in its pre-driver state.
+ */
+static void
+cppc_cpufreq_save_req_and_restore_firmware_regs(struct cpufreq_policy *policy)
+{
+	struct cppc_saved_state *st = cppc_cpufreq_policy_saved_state(policy);
+	unsigned int cpu = policy->cpu;
+	u64 val;
+	int i;
+
+	for (i = 0; i < CPPC_NR_SAVED_REGS; i++) {
+		if (!cppc_saved_regs[i].get(cpu, &val))
+			st[i].requested_val = val;
+		if (st[i].firmware_val != U64_MAX)
+			cppc_saved_regs[i].set(cpu, st[i].firmware_val);
+	}
+}
+
+/*
+ * Reapply each register's requested value that offline() saved.
+ */
+static void cppc_cpufreq_reapply_requested_regs(struct cpufreq_policy *policy)
+{
+	struct cppc_saved_state *st = cppc_cpufreq_policy_saved_state(policy);
+	unsigned int cpu = policy->cpu;
+	int i;
+
+	for (i = 0; i < CPPC_NR_SAVED_REGS; i++)
+		if (st[i].requested_val != U64_MAX)
+			cppc_saved_regs[i].set(cpu, st[i].requested_val);
+}
+
 #ifdef CONFIG_ACPI_CPPC_CPUFREQ_FIE
 static enum {
 	FIE_UNSET = -1,
@@ -707,6 +824,8 @@ static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	policy->cur = cppc_perf_to_khz(caps, caps->highest_perf);
 	cpu_data->perf_ctrls.desired_perf =  caps->highest_perf;
 
+	cppc_cpufreq_save_firmware_regs(policy);
+
 	ret = cppc_set_perf(cpu, &cpu_data->perf_ctrls);
 	if (ret) {
 		pr_debug("Err setting perf value:%d on CPU:%d. ret:%d\n",
@@ -725,15 +844,24 @@ static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 /*
  * With offline() defined, the cpufreq core keeps the policy alive when
  * a CPU is hotplugged out.
+ *
+ * Save each register's current value so online() can reapply it, then restore
+ * the firmware value, leaving the platform in its pre-driver state while the
+ * policy is down (CPU hotplug or driver unload).
  */
 static int cppc_cpufreq_cpu_offline(struct cpufreq_policy *policy)
 {
+	cppc_cpufreq_save_req_and_restore_firmware_regs(policy);
+
 	return 0;
 }
 
 /*
  * Re-enable CPPC when the policy's CPU comes back online, since the platform
  * may have disabled it while the CPU was offline.
+ *
+ * offline() reset the registers to their firmware values, so reapply the
+ * OSPM-set values it saved.
  */
 static int cppc_cpufreq_cpu_online(struct cpufreq_policy *policy)
 {
@@ -744,6 +872,8 @@ static int cppc_cpufreq_cpu_online(struct cpufreq_policy *policy)
 	if (ret && ret != -EOPNOTSUPP)
 		pr_warn("Failed to re-enable CPPC for CPU%d (%d)\n", cpu, ret);
 
+	cppc_cpufreq_reapply_requested_regs(policy);
+
 	return 0;
 }
 
-- 
2.34.1