@@ -105,7 +105,7 @@ static void psci_pd_free_states(struct genpd_power_state *states,
105105 kfree (states );
106106}
107107
108- static int psci_pd_init (struct device_node * np )
108+ static int psci_pd_init (struct device_node * np , bool use_osi )
109109{
110110 struct generic_pm_domain * pd ;
111111 struct psci_pd_provider * pd_provider ;
@@ -135,11 +135,16 @@ static int psci_pd_init(struct device_node *np)
135135
136136 pd -> free_states = psci_pd_free_states ;
137137 pd -> name = kbasename (pd -> name );
138- pd -> power_off = psci_pd_power_off ;
139138 pd -> states = states ;
140139 pd -> state_count = state_count ;
141140 pd -> flags |= GENPD_FLAG_IRQ_SAFE | GENPD_FLAG_CPU_DOMAIN ;
142141
142+ /* Allow power off when OSI has been successfully enabled. */
143+ if (use_osi )
144+ pd -> power_off = psci_pd_power_off ;
145+ else
146+ pd -> flags |= GENPD_FLAG_ALWAYS_ON ;
147+
143148 /* Use governor for CPU PM domains if it has some states to manage. */
144149 pd_gov = state_count > 0 ? & pm_domain_cpu_gov : NULL ;
145150
@@ -190,7 +195,7 @@ static void psci_pd_remove(void)
190195 }
191196}
192197
193- static int psci_pd_init_topology (struct device_node * np , bool add )
198+ static int psci_pd_init_topology (struct device_node * np )
194199{
195200 struct device_node * node ;
196201 struct of_phandle_args child , parent ;
@@ -203,9 +208,7 @@ static int psci_pd_init_topology(struct device_node *np, bool add)
203208
204209 child .np = node ;
205210 child .args_count = 0 ;
206-
207- ret = add ? of_genpd_add_subdomain (& parent , & child ) :
208- of_genpd_remove_subdomain (& parent , & child );
211+ ret = of_genpd_add_subdomain (& parent , & child );
209212 of_node_put (parent .np );
210213 if (ret ) {
211214 of_node_put (node );
@@ -216,14 +219,20 @@ static int psci_pd_init_topology(struct device_node *np, bool add)
216219 return 0 ;
217220}
218221
219- static int psci_pd_add_topology ( struct device_node * np )
222+ static bool psci_pd_try_set_osi_mode ( void )
220223{
221- return psci_pd_init_topology (np , true);
222- }
224+ int ret ;
223225
224- static void psci_pd_remove_topology (struct device_node * np )
225- {
226- psci_pd_init_topology (np , false);
226+ if (!psci_has_osi_support ())
227+ return false;
228+
229+ ret = psci_set_osi_mode (true);
230+ if (ret ) {
231+ pr_warn ("failed to enable OSI mode: %d\n" , ret );
232+ return false;
233+ }
234+
235+ return true;
227236}
228237
229238static void psci_cpuidle_domain_sync_state (struct device * dev )
@@ -244,14 +253,14 @@ static int psci_cpuidle_domain_probe(struct platform_device *pdev)
244253{
245254 struct device_node * np = pdev -> dev .of_node ;
246255 struct device_node * node ;
256+ bool use_osi ;
247257 int ret = 0 , pd_count = 0 ;
248258
249259 if (!np )
250260 return - ENODEV ;
251261
252- /* Currently limit the hierarchical topology to be used in OSI mode. */
253- if (!psci_has_osi_support ())
254- return 0 ;
262+ /* If OSI mode is supported, let's try to enable it. */
263+ use_osi = psci_pd_try_set_osi_mode ();
255264
256265 /*
257266 * Parse child nodes for the "#power-domain-cells" property and
@@ -261,7 +270,7 @@ static int psci_cpuidle_domain_probe(struct platform_device *pdev)
261270 if (!of_find_property (node , "#power-domain-cells" , NULL ))
262271 continue ;
263272
264- ret = psci_pd_init (node );
273+ ret = psci_pd_init (node , use_osi );
265274 if (ret )
266275 goto put_node ;
267276
@@ -270,30 +279,24 @@ static int psci_cpuidle_domain_probe(struct platform_device *pdev)
270279
271280 /* Bail out if not using the hierarchical CPU topology. */
272281 if (!pd_count )
273- return 0 ;
282+ goto no_pd ;
274283
275284 /* Link genpd masters/subdomains to model the CPU topology. */
276- ret = psci_pd_add_topology (np );
285+ ret = psci_pd_init_topology (np );
277286 if (ret )
278287 goto remove_pd ;
279288
280- /* Try to enable OSI mode. */
281- ret = psci_set_osi_mode ();
282- if (ret ) {
283- pr_warn ("failed to enable OSI mode: %d\n" , ret );
284- psci_pd_remove_topology (np );
285- goto remove_pd ;
286- }
287-
288289 pr_info ("Initialized CPU PM domain topology\n" );
289290 return 0 ;
290291
291292put_node :
292293 of_node_put (node );
293294remove_pd :
294- if (pd_count )
295- psci_pd_remove ();
295+ psci_pd_remove ();
296296 pr_err ("failed to create CPU PM domains ret=%d\n" , ret );
297+ no_pd :
298+ if (use_osi )
299+ psci_set_osi_mode (false);
297300 return ret ;
298301}
299302
0 commit comments