[PATCH v2 3/6] cpuidle: invalidate latency gen on per-CPU resume QoS changes

Register a DEV_PM_QOS_RESUME_LATENCY notifier for each CPU device and
bump only that CPU's latency_req generation when its resume latency
constraint changes.

Signed-off-by: Yaxiong Tian <[email protected]>
---
 drivers/cpuidle/cpuidle.c  | 15 +++++++++---
 drivers/cpuidle/cpuidle.h  |  2 ++
 drivers/cpuidle/governor.c | 48 +++++++++++++++++++++++++++++++++++++-
 3 files changed, 61 insertions(+), 4 deletions(-)

diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 2d2f40a2cb81..9495f904fe85 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -613,6 +613,8 @@ static void __cpuidle_unregister_device(struct cpuidle_device *dev)
 {
 	struct cpuidle_driver *drv = cpuidle_get_cpu_driver(dev);
 
+	cpuidle_latency_req_notifier_unregister(dev->cpu);
+
 	list_del(&dev->device_list);
 	per_cpu(cpuidle_devices, dev->cpu) = NULL;
 	module_put(drv->owner);
@@ -661,10 +663,17 @@ static int __cpuidle_register_device(struct cpuidle_device *dev)
 
 	ret = cpuidle_coupled_register_device(dev);
 	if (ret)
-		__cpuidle_unregister_device(dev);
-	else
-		dev->registered = 1;
+		goto unreg;
+
+	ret = cpuidle_latency_req_notifier_register(cpu);
+	if (ret)
+		goto unreg;
 
+	dev->registered = 1;
+	return 0;
+
+unreg:
+	__cpuidle_unregister_device(dev);
 	return ret;
 }
 
diff --git a/drivers/cpuidle/cpuidle.h b/drivers/cpuidle/cpuidle.h
index 52701d9588f1..151fa9ebe483 100644
--- a/drivers/cpuidle/cpuidle.h
+++ b/drivers/cpuidle/cpuidle.h
@@ -25,6 +25,8 @@ extern void cpuidle_uninstall_idle_handler(void);
 /* governors */
 extern struct cpuidle_governor *cpuidle_find_governor(const char *str);
 extern int cpuidle_switch_governor(struct cpuidle_governor *gov);
+int cpuidle_latency_req_notifier_register(unsigned int cpu);
+void cpuidle_latency_req_notifier_unregister(unsigned int cpu);
 
 /* sysfs */
 
diff --git a/drivers/cpuidle/governor.c b/drivers/cpuidle/governor.c
index bc4a70d30c34..d286ccf19a69 100644
--- a/drivers/cpuidle/governor.c
+++ b/drivers/cpuidle/governor.c
@@ -27,10 +27,18 @@ struct cpuidle_governor *cpuidle_prev_governor;
 
 /*
  * Per-CPU generation bumped to invalidate that CPU's cached latency
- * constraint.  Consumers of the generation are added in later changes.
+ * constraint.  Global QoS changes invalidate every CPU; per-CPU resume
+ * latency changes invalidate only the affected CPU.
  */
 static DEFINE_PER_CPU(atomic_t, latency_req_gen);
 
+struct cpuidle_cpu_qos_nb {
+	struct notifier_block nb;
+	unsigned int cpu;
+};
+
+static DEFINE_PER_CPU(struct cpuidle_cpu_qos_nb, cpuidle_cpu_qos_nb);
+
 static void cpuidle_latency_req_invalidate_cpu(unsigned int cpu)
 {
 	atomic_inc(per_cpu_ptr(&latency_req_gen, cpu));
@@ -61,6 +69,44 @@ static struct notifier_block cpuidle_wakeup_qos_nb = {
 };
 #endif
 
+static int cpuidle_cpu_qos_notify(struct notifier_block *nb,
+				 unsigned long action, void *data)
+{
+	struct cpuidle_cpu_qos_nb *qos_nb =
+		container_of(nb, struct cpuidle_cpu_qos_nb, nb);
+
+	cpuidle_latency_req_invalidate_cpu(qos_nb->cpu);
+	return NOTIFY_OK;
+}
+
+int cpuidle_latency_req_notifier_register(unsigned int cpu)
+{
+	struct device *device = get_cpu_device(cpu);
+	struct cpuidle_cpu_qos_nb *qos_nb =
+		per_cpu_ptr(&cpuidle_cpu_qos_nb, cpu);
+
+	if (!device)
+		return -ENODEV;
+
+	qos_nb->cpu = cpu;
+	qos_nb->nb.notifier_call = cpuidle_cpu_qos_notify;
+	return dev_pm_qos_add_notifier(device, &qos_nb->nb,
+				       DEV_PM_QOS_RESUME_LATENCY);
+}
+
+void cpuidle_latency_req_notifier_unregister(unsigned int cpu)
+{
+	struct device *device = get_cpu_device(cpu);
+	struct cpuidle_cpu_qos_nb *qos_nb =
+		per_cpu_ptr(&cpuidle_cpu_qos_nb, cpu);
+
+	if (!device)
+		return;
+
+	dev_pm_qos_remove_notifier(device, &qos_nb->nb,
+				   DEV_PM_QOS_RESUME_LATENCY);
+}
+
 static int __init cpuidle_latency_req_init(void)
 {
 	int ret;
-- 
2.43.0