cpuidle: psci

- Fix the error path to prevent reverting from OSI back to PC mode
 -----BEGIN PGP SIGNATURE-----
 
 iQJLBAABCgA1FiEEugLDXPmKSktSkQsV/iaEJXNYjCkFAmTWEnwXHHVsZi5oYW5z
 c29uQGxpbmFyby5vcmcACgkQ/iaEJXNYjCnhEA/+P0fFhNNlSWD9oZLDf+Ehe71z
 i2nA+n0vvjWQZqUfFtTlrc/3umtMSqxvM7G0ojItruJhqKJp1PqA+3WowF+cyR1r
 0o2ynpuSbNPPthGa+Fyw/2n5LXHih9odkNfXhugUbghkZiE7DTUaOZO7h0hajXjE
 VMKfgoaig5iIJOfdxiHs8PyRvWxqQvJeYzG9ciZgxkW1O/eJi0u5ftP6Tvp6i7rc
 GmpqOF/aRoNU4S+anxxI0ovfVb0mzAE9A46lSWBErHXfBPYm4zntVkwxjcI8U1Gk
 ICuYrMdkqQilJjz+Tib2wrL5ClqszoIS1NOr+7hAyXKd9RFWIf8E0K9aZ9qH353e
 jKG1NgekgG07r07G/31yX6Nv/EOfeVlv/0V1V9qnEhnbf8otlT2pjk4l3oxWwGIo
 wxrvx6zLSKWQGDxzkzIhtzLXqG/QMIiXW2py/s4D3oK2cQZRtb8IwfnR6rRj7rla
 DcEA50Tb2BDW97rmumB4haGe6RS0fduKzX0kb+bLzYAOpcW5/n53RweAQUl6kb0n
 CnJEMyC/8MFD+FXWrdCz+uUclJh+sQOZumPKzgN2FlY/RolBdfbGssb20gYKHkBZ
 dwo8GcxsDdNxuZQy+Xdiyqr8fsWggH74F4XiVQm2dyvDSRv9oy+so6gSHoWnVIEZ
 D1UGjuXiTAsaxEZ2bQ4=
 =rDCc
 -----END PGP SIGNATURE-----

Merge tag 'cpuidle-psci-v6.5-rc5' of git://git.kernel.org/pub/scm/linux/kernel/git/ulfh/linux-pm

Pull cpuidle psci fixes from Ulf Hansson:
 "A couple of cpuidle-psci fixes. Usually, this is managed by arm-soc
  maintainers or Rafael, although due to a busy period I have stepped in
  to help out:

   - Fix the error path to prevent reverting from OSI back to PC mode"

* tag 'cpuidle-psci-v6.5-rc5' of git://git.kernel.org/pub/scm/linux/kernel/git/ulfh/linux-pm:
  cpuidle: psci: Move enabling OSI mode after power domains creation
  cpuidle: dt_idle_genpd: Add helper function to remove genpd topology

drivers/cpuidle/cpuidle-psci-domain.c

@@ -120,20 +120,6 @@ static void psci_pd_remove(void)
 	}
 }
 
-static bool psci_pd_try_set_osi_mode(void)
-{
-	int ret;
-
-	if (!psci_has_osi_support())
-		return false;
-
-	ret = psci_set_osi_mode(true);
-	if (ret)
-		return false;
-
-	return true;
-}
-
 static void psci_cpuidle_domain_sync_state(struct device *dev)
 {
 	/*
@@ -152,15 +138,12 @@ static int psci_cpuidle_domain_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct device_node *node;
-	bool use_osi;
+	bool use_osi = psci_has_osi_support();
 	int ret = 0, pd_count = 0;
 
 	if (!np)
 		return -ENODEV;
 
-	/* If OSI mode is supported, let's try to enable it. */
-	use_osi = psci_pd_try_set_osi_mode();
-
 	/*
 	 * Parse child nodes for the "#power-domain-cells" property and
 	 * initialize a genpd/genpd-of-provider pair when it's found.
@@ -170,33 +153,37 @@ static int psci_cpuidle_domain_probe(struct platform_device *pdev)
 			continue;
 
 		ret = psci_pd_init(node, use_osi);
-		if (ret)
-			goto put_node;
+		if (ret) {
+			of_node_put(node);
+			goto exit;
+		}
 
 		pd_count++;
 	}
 
 	/* Bail out if not using the hierarchical CPU topology. */
 	if (!pd_count)
-		goto no_pd;
+		return 0;
 
 	/* Link genpd masters/subdomains to model the CPU topology. */
 	ret = dt_idle_pd_init_topology(np);
 	if (ret)
 		goto remove_pd;
 
+	/* let's try to enable OSI. */
+	ret = psci_set_osi_mode(use_osi);
+	if (ret)
+		goto remove_pd;
+
 	pr_info("Initialized CPU PM domain topology using %s mode\n",
 		use_osi ? "OSI" : "PC");
 	return 0;
 
-put_node:
-	of_node_put(node);
 remove_pd:
+	dt_idle_pd_remove_topology(np);
 	psci_pd_remove();
+exit:
 	pr_err("failed to create CPU PM domains ret=%d\n", ret);
-no_pd:
-	if (use_osi)
-		psci_set_osi_mode(false);
 	return ret;
 }
 

drivers/cpuidle/dt_idle_genpd.c

@@ -152,6 +152,30 @@ int dt_idle_pd_init_topology(struct device_node *np)
 	return 0;
 }
 
+int dt_idle_pd_remove_topology(struct device_node *np)
+{
+	struct device_node *node;
+	struct of_phandle_args child, parent;
+	int ret;
+
+	for_each_child_of_node(np, node) {
+		if (of_parse_phandle_with_args(node, "power-domains",
+					"#power-domain-cells", 0, &parent))
+			continue;
+
+		child.np = node;
+		child.args_count = 0;
+		ret = of_genpd_remove_subdomain(&parent, &child);
+		of_node_put(parent.np);
+		if (ret) {
+			of_node_put(node);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
 struct device *dt_idle_attach_cpu(int cpu, const char *name)
 {
 	struct device *dev;

drivers/cpuidle/dt_idle_genpd.h

@@ -14,6 +14,8 @@ struct generic_pm_domain *dt_idle_pd_alloc(struct device_node *np,
 
 int dt_idle_pd_init_topology(struct device_node *np);
 
+int dt_idle_pd_remove_topology(struct device_node *np);
+
 struct device *dt_idle_attach_cpu(int cpu, const char *name);
 
 void dt_idle_detach_cpu(struct device *dev);
@@ -36,6 +38,11 @@ static inline int dt_idle_pd_init_topology(struct device_node *np)
 	return 0;
 }
 
+static inline int dt_idle_pd_remove_topology(struct device_node *np)
+{
+	return 0;
+}
+
 static inline struct device *dt_idle_attach_cpu(int cpu, const char *name)
 {
 	return NULL;