[09/18] drivers: firmware: psci: Add support for PM domains using genpd
diff mbox series

Message ID 20190513192300.653-10-ulf.hansson@linaro.org
State Not Applicable, archived
Headers show
Series
  • ARM/ARM64: Support hierarchical CPU arrangement for PSCI
Related show

Commit Message

Ulf Hansson May 13, 2019, 7:22 p.m. UTC
When the hierarchical CPU topology layout is used in DT, we need to setup
the corresponding PM domain data structures, as to allow a CPU and a group
of CPUs to be power managed accordingly. Let's enable this by deploying
support through the genpd interface.

Additionally, when the OS initiated mode is supported by the PSCI FW, let's
also parse the domain idle states DT bindings as to make genpd responsible
for the state selection, when the states are compatible with
"domain-idle-state". Otherwise, when only Platform Coordinated mode is
supported, we rely solely on the state selection to be managed through the
regular cpuidle framework.

If the initialization of the PM domain data structures succeeds and the OS
initiated mode is supported, we try to switch to it. In case it fails,
let's fall back into a degraded mode, rather than bailing out and returning
an error code.

Due to that the OS initiated mode may become enabled, we need to adjust to
maintain backwards compatibility for a kernel started through a kexec call.
Do this by explicitly switch to Platform Coordinated mode during boot.

Finally, the actual initialization of the PM domain data structures, is
done via calling the new shared function, psci_dt_init_pm_domains().
However, this is implemented by subsequent changes.

Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
---

Changes:
	- Simplify code setting domain_state at power off.
	- Use the genpd ->free_state() callback to manage freeing of states.
	- Fixup a bogus while loop.

---
 drivers/firmware/psci/Makefile         |   2 +-
 drivers/firmware/psci/psci.c           |   7 +-
 drivers/firmware/psci/psci.h           |   5 +
 drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
 4 files changed, 280 insertions(+), 2 deletions(-)
 create mode 100644 drivers/firmware/psci/psci_pm_domain.c

Comments

Sudeep Holla June 7, 2019, 3:27 p.m. UTC | #1
On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
> When the hierarchical CPU topology layout is used in DT, we need to setup
> the corresponding PM domain data structures, as to allow a CPU and a group
> of CPUs to be power managed accordingly. Let's enable this by deploying
> support through the genpd interface.
> 
> Additionally, when the OS initiated mode is supported by the PSCI FW, let's
> also parse the domain idle states DT bindings as to make genpd responsible
> for the state selection, when the states are compatible with
> "domain-idle-state". Otherwise, when only Platform Coordinated mode is
> supported, we rely solely on the state selection to be managed through the
> regular cpuidle framework.
> 
> If the initialization of the PM domain data structures succeeds and the OS
> initiated mode is supported, we try to switch to it. In case it fails,
> let's fall back into a degraded mode, rather than bailing out and returning
> an error code.
> 
> Due to that the OS initiated mode may become enabled, we need to adjust to
> maintain backwards compatibility for a kernel started through a kexec call.
> Do this by explicitly switch to Platform Coordinated mode during boot.
> 
> Finally, the actual initialization of the PM domain data structures, is
> done via calling the new shared function, psci_dt_init_pm_domains().
> However, this is implemented by subsequent changes.
> 
> Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
> Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
> Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
> ---
> 
> Changes:
> 	- Simplify code setting domain_state at power off.
> 	- Use the genpd ->free_state() callback to manage freeing of states.
> 	- Fixup a bogus while loop.
> 
> ---
>  drivers/firmware/psci/Makefile         |   2 +-
>  drivers/firmware/psci/psci.c           |   7 +-
>  drivers/firmware/psci/psci.h           |   5 +
>  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
>  4 files changed, 280 insertions(+), 2 deletions(-)
>  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
> 
> diff --git a/drivers/firmware/psci/Makefile b/drivers/firmware/psci/Makefile
> index 1956b882470f..ff300f1fec86 100644
> --- a/drivers/firmware/psci/Makefile
> +++ b/drivers/firmware/psci/Makefile
> @@ -1,4 +1,4 @@
>  # SPDX-License-Identifier: GPL-2.0
>  #
> -obj-$(CONFIG_ARM_PSCI_FW)	+= psci.o
> +obj-$(CONFIG_ARM_PSCI_FW)	+= psci.o psci_pm_domain.o
>  obj-$(CONFIG_ARM_PSCI_CHECKER)	+= psci_checker.o
> diff --git a/drivers/firmware/psci/psci.c b/drivers/firmware/psci/psci.c
> index 0e91d864e346..bfef300b7ebe 100644
> --- a/drivers/firmware/psci/psci.c
> +++ b/drivers/firmware/psci/psci.c
> @@ -721,9 +721,14 @@ static int __init psci_1_0_init(struct device_node *np)
>  	if (err)
>  		return err;
>  
> -	if (psci_has_osi_support())
> +	if (psci_has_osi_support()) {
>  		pr_info("OSI mode supported.\n");
>  
> +		/* Make sure we default to PC mode. */
> +		invoke_psci_fn(PSCI_1_0_FN_SET_SUSPEND_MODE,
> +			       PSCI_1_0_SUSPEND_MODE_PC, 0, 0);
> +	}
> +

This can go on it's own, any issues with that ?

>  	return 0;
>  }
>  
> diff --git a/drivers/firmware/psci/psci.h b/drivers/firmware/psci/psci.h
> index f2277c3ad405..00d2e3dcef49 100644
> --- a/drivers/firmware/psci/psci.h
> +++ b/drivers/firmware/psci/psci.h
> @@ -11,6 +11,11 @@ bool psci_has_osi_support(void);
>  #ifdef CONFIG_CPU_IDLE
>  void psci_set_domain_state(u32 state);
>  int psci_dt_parse_state_node(struct device_node *np, u32 *state);
> +#ifdef CONFIG_PM_GENERIC_DOMAINS_OF
> +int psci_dt_init_pm_domains(struct device_node *np);
> +#else
> +static inline int psci_dt_init_pm_domains(struct device_node *np) { return 0; }
> +#endif
>  #endif
>  
>  #endif /* __PSCI_H */
> diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
> new file mode 100644
> index 000000000000..3c6ca846caf4
> --- /dev/null
> +++ b/drivers/firmware/psci/psci_pm_domain.c
> @@ -0,0 +1,268 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * PM domains for CPUs via genpd - managed by PSCI.
> + *
> + * Copyright (C) 2019 Linaro Ltd.
> + * Author: Ulf Hansson <ulf.hansson@linaro.org>
> + *
> + */
> +
> +#define pr_fmt(fmt) "psci: " fmt
> +
> +#include <linux/device.h>
> +#include <linux/kernel.h>
> +#include <linux/pm_domain.h>
> +#include <linux/slab.h>
> +#include <linux/string.h>
> +
> +#include "psci.h"
> +
> +#if defined(CONFIG_CPU_IDLE) && defined(CONFIG_PM_GENERIC_DOMAINS_OF)
> +
> +struct psci_pd_provider {
> +	struct list_head link;
> +	struct device_node *node;
> +};
> +
> +static LIST_HEAD(psci_pd_providers);
> +static bool osi_mode_enabled;
> +
> +static int psci_pd_power_off(struct generic_pm_domain *pd)
> +{
> +	struct genpd_power_state *state = &pd->states[pd->state_idx];
> +	u32 *pd_state;
> +
> +	/* If we have failed to enable OSI mode, then abort power off. */
> +	if (psci_has_osi_support() && !osi_mode_enabled)
> +		return -EBUSY;
> +
> +	if (!state->data)
> +		return 0;
> +
> +	/* When OSI mode is enabled, set the corresponding domain state. */
> +	pd_state = state->data;
> +	psci_set_domain_state(*pd_state);
> +
> +	return 0;
> +}
> +
> +static int psci_pd_parse_state_nodes(struct genpd_power_state *states,
> +				int state_count)
> +{
> +	int i, ret;
> +	u32 psci_state, *psci_state_buf;
> +
> +	for (i = 0; i < state_count; i++) {
> +		ret = psci_dt_parse_state_node(to_of_node(states[i].fwnode),
> +					&psci_state);
> +		if (ret)
> +			goto free_state;
> +
> +		psci_state_buf = kmalloc(sizeof(u32), GFP_KERNEL);
> +		if (!psci_state_buf) {
> +			ret = -ENOMEM;
> +			goto free_state;
> +		}
> +		*psci_state_buf = psci_state;
> +		states[i].data = psci_state_buf;
> +	}
> +
> +	return 0;
> +
> +free_state:
> +	i--;
> +	for (; i >= 0; i--)
> +		kfree(states[i].data);
> +	return ret;
> +}
> +
> +static int psci_pd_parse_states(struct device_node *np,
> +			struct genpd_power_state **states, int *state_count)
> +{
> +	int ret;
> +
> +	/* Parse the domain idle states. */
> +	ret = of_genpd_parse_idle_states(np, states, state_count);
> +	if (ret)
> +		return ret;
> +


Lots of things here in this file are not psci specific. They can be
moved as generic CPU PM domain support.

> +	/* Fill out the PSCI specifics for each found state. */
> +	ret = psci_pd_parse_state_nodes(*states, *state_count);
> +	if (ret)
> +		kfree(*states);
> +

Things like above are PSCI.

I am trying to see if we can do something to achieve partitions like we
have today: psci.c just has PSCI specific stuff and dt_idle_states.c
deals with generic idle stuff.

--
Regards,
Sudeep
Ulf Hansson June 10, 2019, 10:21 a.m. UTC | #2
On Fri, 7 Jun 2019 at 17:27, Sudeep Holla <sudeep.holla@arm.com> wrote:
>
> On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
> > When the hierarchical CPU topology layout is used in DT, we need to setup
> > the corresponding PM domain data structures, as to allow a CPU and a group
> > of CPUs to be power managed accordingly. Let's enable this by deploying
> > support through the genpd interface.
> >
> > Additionally, when the OS initiated mode is supported by the PSCI FW, let's
> > also parse the domain idle states DT bindings as to make genpd responsible
> > for the state selection, when the states are compatible with
> > "domain-idle-state". Otherwise, when only Platform Coordinated mode is
> > supported, we rely solely on the state selection to be managed through the
> > regular cpuidle framework.
> >
> > If the initialization of the PM domain data structures succeeds and the OS
> > initiated mode is supported, we try to switch to it. In case it fails,
> > let's fall back into a degraded mode, rather than bailing out and returning
> > an error code.
> >
> > Due to that the OS initiated mode may become enabled, we need to adjust to
> > maintain backwards compatibility for a kernel started through a kexec call.
> > Do this by explicitly switch to Platform Coordinated mode during boot.
> >
> > Finally, the actual initialization of the PM domain data structures, is
> > done via calling the new shared function, psci_dt_init_pm_domains().
> > However, this is implemented by subsequent changes.
> >
> > Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
> > Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
> > Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
> > ---
> >
> > Changes:
> >       - Simplify code setting domain_state at power off.
> >       - Use the genpd ->free_state() callback to manage freeing of states.
> >       - Fixup a bogus while loop.
> >
> > ---
> >  drivers/firmware/psci/Makefile         |   2 +-
> >  drivers/firmware/psci/psci.c           |   7 +-
> >  drivers/firmware/psci/psci.h           |   5 +
> >  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
> >  4 files changed, 280 insertions(+), 2 deletions(-)
> >  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
> >
> > diff --git a/drivers/firmware/psci/Makefile b/drivers/firmware/psci/Makefile
> > index 1956b882470f..ff300f1fec86 100644
> > --- a/drivers/firmware/psci/Makefile
> > +++ b/drivers/firmware/psci/Makefile
> > @@ -1,4 +1,4 @@
> >  # SPDX-License-Identifier: GPL-2.0
> >  #
> > -obj-$(CONFIG_ARM_PSCI_FW)    += psci.o
> > +obj-$(CONFIG_ARM_PSCI_FW)    += psci.o psci_pm_domain.o
> >  obj-$(CONFIG_ARM_PSCI_CHECKER)       += psci_checker.o
> > diff --git a/drivers/firmware/psci/psci.c b/drivers/firmware/psci/psci.c
> > index 0e91d864e346..bfef300b7ebe 100644
> > --- a/drivers/firmware/psci/psci.c
> > +++ b/drivers/firmware/psci/psci.c
> > @@ -721,9 +721,14 @@ static int __init psci_1_0_init(struct device_node *np)
> >       if (err)
> >               return err;
> >
> > -     if (psci_has_osi_support())
> > +     if (psci_has_osi_support()) {
> >               pr_info("OSI mode supported.\n");
> >
> > +             /* Make sure we default to PC mode. */
> > +             invoke_psci_fn(PSCI_1_0_FN_SET_SUSPEND_MODE,
> > +                            PSCI_1_0_SUSPEND_MODE_PC, 0, 0);
> > +     }
> > +
>
> This can go on it's own, any issues with that ?

Sure, I can move that out to a separate patch.

>
> >       return 0;
> >  }
> >
> > diff --git a/drivers/firmware/psci/psci.h b/drivers/firmware/psci/psci.h
> > index f2277c3ad405..00d2e3dcef49 100644
> > --- a/drivers/firmware/psci/psci.h
> > +++ b/drivers/firmware/psci/psci.h
> > @@ -11,6 +11,11 @@ bool psci_has_osi_support(void);
> >  #ifdef CONFIG_CPU_IDLE
> >  void psci_set_domain_state(u32 state);
> >  int psci_dt_parse_state_node(struct device_node *np, u32 *state);
> > +#ifdef CONFIG_PM_GENERIC_DOMAINS_OF
> > +int psci_dt_init_pm_domains(struct device_node *np);
> > +#else
> > +static inline int psci_dt_init_pm_domains(struct device_node *np) { return 0; }
> > +#endif
> >  #endif
> >
> >  #endif /* __PSCI_H */
> > diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
> > new file mode 100644
> > index 000000000000..3c6ca846caf4
> > --- /dev/null
> > +++ b/drivers/firmware/psci/psci_pm_domain.c
> > @@ -0,0 +1,268 @@
> > +// SPDX-License-Identifier: GPL-2.0
> > +/*
> > + * PM domains for CPUs via genpd - managed by PSCI.
> > + *
> > + * Copyright (C) 2019 Linaro Ltd.
> > + * Author: Ulf Hansson <ulf.hansson@linaro.org>
> > + *
> > + */
> > +
> > +#define pr_fmt(fmt) "psci: " fmt
> > +
> > +#include <linux/device.h>
> > +#include <linux/kernel.h>
> > +#include <linux/pm_domain.h>
> > +#include <linux/slab.h>
> > +#include <linux/string.h>
> > +
> > +#include "psci.h"
> > +
> > +#if defined(CONFIG_CPU_IDLE) && defined(CONFIG_PM_GENERIC_DOMAINS_OF)
> > +
> > +struct psci_pd_provider {
> > +     struct list_head link;
> > +     struct device_node *node;
> > +};
> > +
> > +static LIST_HEAD(psci_pd_providers);
> > +static bool osi_mode_enabled;
> > +
> > +static int psci_pd_power_off(struct generic_pm_domain *pd)
> > +{
> > +     struct genpd_power_state *state = &pd->states[pd->state_idx];
> > +     u32 *pd_state;
> > +
> > +     /* If we have failed to enable OSI mode, then abort power off. */
> > +     if (psci_has_osi_support() && !osi_mode_enabled)
> > +             return -EBUSY;
> > +
> > +     if (!state->data)
> > +             return 0;
> > +
> > +     /* When OSI mode is enabled, set the corresponding domain state. */
> > +     pd_state = state->data;
> > +     psci_set_domain_state(*pd_state);
> > +
> > +     return 0;
> > +}
> > +
> > +static int psci_pd_parse_state_nodes(struct genpd_power_state *states,
> > +                             int state_count)
> > +{
> > +     int i, ret;
> > +     u32 psci_state, *psci_state_buf;
> > +
> > +     for (i = 0; i < state_count; i++) {
> > +             ret = psci_dt_parse_state_node(to_of_node(states[i].fwnode),
> > +                                     &psci_state);
> > +             if (ret)
> > +                     goto free_state;
> > +
> > +             psci_state_buf = kmalloc(sizeof(u32), GFP_KERNEL);
> > +             if (!psci_state_buf) {
> > +                     ret = -ENOMEM;
> > +                     goto free_state;
> > +             }
> > +             *psci_state_buf = psci_state;
> > +             states[i].data = psci_state_buf;
> > +     }
> > +
> > +     return 0;
> > +
> > +free_state:
> > +     i--;
> > +     for (; i >= 0; i--)
> > +             kfree(states[i].data);
> > +     return ret;
> > +}
> > +
> > +static int psci_pd_parse_states(struct device_node *np,
> > +                     struct genpd_power_state **states, int *state_count)
> > +{
> > +     int ret;
> > +
> > +     /* Parse the domain idle states. */
> > +     ret = of_genpd_parse_idle_states(np, states, state_count);
> > +     if (ret)
> > +             return ret;
> > +
>
>
> Lots of things here in this file are not psci specific. They can be
> moved as generic CPU PM domain support.

What exactly do you mean by CPU PM domain support?

The current split is based upon how the generic PM domain (genpd)
supports CPU devices (see GENPD_FLAG_CPU_DOMAIN), which is already
available.

I agree that finding the right balance between what can be made
generic and driver specific is not always obvious. Often it's better
to start with having more things in the driver code, then move things
into a common framework, later on, when that turns out to make sense.

>
> > +     /* Fill out the PSCI specifics for each found state. */
> > +     ret = psci_pd_parse_state_nodes(*states, *state_count);
> > +     if (ret)
> > +             kfree(*states);
> > +
>
> Things like above are PSCI.
>
> I am trying to see if we can do something to achieve partitions like we
> have today: psci.c just has PSCI specific stuff and dt_idle_states.c
> deals with generic idle stuff.

I am open to any suggestions. Although, I am not sure I understand
your comment and nor the reason to why you want me to change.

So, what is the problem with having the code that you refer to, inside
drivers/firmware/psci/psci_pm_domain.c? Can't we just start with that
and see how it plays?

Kind regards
Uffe
Sudeep Holla June 10, 2019, 10:59 a.m. UTC | #3
On Mon, Jun 10, 2019 at 12:21:41PM +0200, Ulf Hansson wrote:
> On Fri, 7 Jun 2019 at 17:27, Sudeep Holla <sudeep.holla@arm.com> wrote:
> >
> > On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
> > > When the hierarchical CPU topology layout is used in DT, we need to setup
> > > the corresponding PM domain data structures, as to allow a CPU and a group
> > > of CPUs to be power managed accordingly. Let's enable this by deploying
> > > support through the genpd interface.
> > >
> > > Additionally, when the OS initiated mode is supported by the PSCI FW, let's
> > > also parse the domain idle states DT bindings as to make genpd responsible
> > > for the state selection, when the states are compatible with
> > > "domain-idle-state". Otherwise, when only Platform Coordinated mode is
> > > supported, we rely solely on the state selection to be managed through the
> > > regular cpuidle framework.
> > >
> > > If the initialization of the PM domain data structures succeeds and the OS
> > > initiated mode is supported, we try to switch to it. In case it fails,
> > > let's fall back into a degraded mode, rather than bailing out and returning
> > > an error code.
> > >
> > > Due to that the OS initiated mode may become enabled, we need to adjust to
> > > maintain backwards compatibility for a kernel started through a kexec call.
> > > Do this by explicitly switch to Platform Coordinated mode during boot.
> > >
> > > Finally, the actual initialization of the PM domain data structures, is
> > > done via calling the new shared function, psci_dt_init_pm_domains().
> > > However, this is implemented by subsequent changes.
> > >
> > > Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
> > > Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
> > > Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
> > > ---
> > >
> > > Changes:
> > >       - Simplify code setting domain_state at power off.
> > >       - Use the genpd ->free_state() callback to manage freeing of states.
> > >       - Fixup a bogus while loop.
> > >
> > > ---
> > >  drivers/firmware/psci/Makefile         |   2 +-
> > >  drivers/firmware/psci/psci.c           |   7 +-
> > >  drivers/firmware/psci/psci.h           |   5 +
> > >  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
> > >  4 files changed, 280 insertions(+), 2 deletions(-)
> > >  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
> > >

[...]

> > > +
> > > +static int psci_pd_parse_states(struct device_node *np,
> > > +                     struct genpd_power_state **states, int *state_count)
> > > +{
> > > +     int ret;
> > > +
> > > +     /* Parse the domain idle states. */
> > > +     ret = of_genpd_parse_idle_states(np, states, state_count);
> > > +     if (ret)
> > > +             return ret;
> > > +
> >
> >
> > Lots of things here in this file are not psci specific. They can be
> > moved as generic CPU PM domain support.
>
> What exactly do you mean by CPU PM domain support?
>
> The current split is based upon how the generic PM domain (genpd)
> supports CPU devices (see GENPD_FLAG_CPU_DOMAIN), which is already
> available.
>
> I agree that finding the right balance between what can be made
> generic and driver specific is not always obvious. Often it's better
> to start with having more things in the driver code, then move things
> into a common framework, later on, when that turns out to make sense.
>

Indeed, I agree. But when reviewing this time I thought it should be
possible to push generic stuff into existing dt_idle_driver. I must
admit that I haven't thought much in details, just thought of expressing
the idea and see. But yes it's difficult to find the balance but at the
same time we need reasons to have these in psci :)


> >
> > > +     /* Fill out the PSCI specifics for each found state. */
> > > +     ret = psci_pd_parse_state_nodes(*states, *state_count);
> > > +     if (ret)
> > > +             kfree(*states);
> > > +
> >
> > Things like above are PSCI.
> >
> > I am trying to see if we can do something to achieve partitions like we
> > have today: psci.c just has PSCI specific stuff and dt_idle_states.c
> > deals with generic idle stuff.
> 
> I am open to any suggestions. Although, I am not sure I understand
> your comment and nor the reason to why you want me to change.
> 
> So, what is the problem with having the code that you refer to, inside
> drivers/firmware/psci/psci_pm_domain.c? Can't we just start with that
> and see how it plays?
> 

I need to think how to partition this well. I don't have suggestions
right away, but I need to get convinced what we have here is best we
can do or come up with a better solution. I didn't like it as is at
this time.

--
Regards,
Sudeep
Sudeep Holla July 16, 2019, 3:05 p.m. UTC | #4
On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
> When the hierarchical CPU topology layout is used in DT, we need to setup
> the corresponding PM domain data structures, as to allow a CPU and a group
> of CPUs to be power managed accordingly. Let's enable this by deploying
> support through the genpd interface.
>
> Additionally, when the OS initiated mode is supported by the PSCI FW, let's
> also parse the domain idle states DT bindings as to make genpd responsible
> for the state selection, when the states are compatible with
> "domain-idle-state". Otherwise, when only Platform Coordinated mode is
> supported, we rely solely on the state selection to be managed through the
> regular cpuidle framework.
>
> If the initialization of the PM domain data structures succeeds and the OS
> initiated mode is supported, we try to switch to it. In case it fails,
> let's fall back into a degraded mode, rather than bailing out and returning
> an error code.
>
> Due to that the OS initiated mode may become enabled, we need to adjust to
> maintain backwards compatibility for a kernel started through a kexec call.
> Do this by explicitly switch to Platform Coordinated mode during boot.
>
> Finally, the actual initialization of the PM domain data structures, is
> done via calling the new shared function, psci_dt_init_pm_domains().
> However, this is implemented by subsequent changes.
>
> Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
> Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
> Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
> ---
>
> Changes:
> 	- Simplify code setting domain_state at power off.
> 	- Use the genpd ->free_state() callback to manage freeing of states.
> 	- Fixup a bogus while loop.
>
> ---
>  drivers/firmware/psci/Makefile         |   2 +-
>  drivers/firmware/psci/psci.c           |   7 +-
>  drivers/firmware/psci/psci.h           |   5 +
>  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
>  4 files changed, 280 insertions(+), 2 deletions(-)
>  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
>

[...]

>  #endif /* __PSCI_H */
> diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
> new file mode 100644
> index 000000000000..3c6ca846caf4
> --- /dev/null
> +++ b/drivers/firmware/psci/psci_pm_domain.c
> @@ -0,0 +1,268 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * PM domains for CPUs via genpd - managed by PSCI.
> + *
> + * Copyright (C) 2019 Linaro Ltd.
> + * Author: Ulf Hansson <ulf.hansson@linaro.org>
> + *
> + */
> +

[...]

> +static int psci_pd_power_off(struct generic_pm_domain *pd)
> +{
> +	struct genpd_power_state *state = &pd->states[pd->state_idx];
> +	u32 *pd_state;
> +
> +	/* If we have failed to enable OSI mode, then abort power off. */
> +	if (psci_has_osi_support() && !osi_mode_enabled)
> +		return -EBUSY;
> +
> +	if (!state->data)
> +		return 0;
> +
> +	/* When OSI mode is enabled, set the corresponding domain state. */
> +	pd_state = state->data;
> +	psci_set_domain_state(*pd_state);

I trying to understand how would this scale to level 2(cluster of
clusters or for simply system). The current code for psci_set_domain_state
just stores the value @pd_state into per-cpu domain_state. E.g.: Now if
the system level pd is getting called after cluster PD, it will set the
domain state to system level PD state. It won't work with original
format and it may work with extended format if it's carefully crafted.
In short, the point is just over-writing domain_state is asking for
troubles IMO.

--
Regards,
Sudeep
Ulf Hansson July 18, 2019, 11:04 a.m. UTC | #5
On Tue, 16 Jul 2019 at 17:05, Sudeep Holla <sudeep.holla@arm.com> wrote:
>
> On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
> > When the hierarchical CPU topology layout is used in DT, we need to setup
> > the corresponding PM domain data structures, as to allow a CPU and a group
> > of CPUs to be power managed accordingly. Let's enable this by deploying
> > support through the genpd interface.
> >
> > Additionally, when the OS initiated mode is supported by the PSCI FW, let's
> > also parse the domain idle states DT bindings as to make genpd responsible
> > for the state selection, when the states are compatible with
> > "domain-idle-state". Otherwise, when only Platform Coordinated mode is
> > supported, we rely solely on the state selection to be managed through the
> > regular cpuidle framework.
> >
> > If the initialization of the PM domain data structures succeeds and the OS
> > initiated mode is supported, we try to switch to it. In case it fails,
> > let's fall back into a degraded mode, rather than bailing out and returning
> > an error code.
> >
> > Due to that the OS initiated mode may become enabled, we need to adjust to
> > maintain backwards compatibility for a kernel started through a kexec call.
> > Do this by explicitly switch to Platform Coordinated mode during boot.
> >
> > Finally, the actual initialization of the PM domain data structures, is
> > done via calling the new shared function, psci_dt_init_pm_domains().
> > However, this is implemented by subsequent changes.
> >
> > Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
> > Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
> > Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
> > ---
> >
> > Changes:
> >       - Simplify code setting domain_state at power off.
> >       - Use the genpd ->free_state() callback to manage freeing of states.
> >       - Fixup a bogus while loop.
> >
> > ---
> >  drivers/firmware/psci/Makefile         |   2 +-
> >  drivers/firmware/psci/psci.c           |   7 +-
> >  drivers/firmware/psci/psci.h           |   5 +
> >  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
> >  4 files changed, 280 insertions(+), 2 deletions(-)
> >  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
> >
>
> [...]
>
> >  #endif /* __PSCI_H */
> > diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
> > new file mode 100644
> > index 000000000000..3c6ca846caf4
> > --- /dev/null
> > +++ b/drivers/firmware/psci/psci_pm_domain.c
> > @@ -0,0 +1,268 @@
> > +// SPDX-License-Identifier: GPL-2.0
> > +/*
> > + * PM domains for CPUs via genpd - managed by PSCI.
> > + *
> > + * Copyright (C) 2019 Linaro Ltd.
> > + * Author: Ulf Hansson <ulf.hansson@linaro.org>
> > + *
> > + */
> > +
>
> [...]
>
> > +static int psci_pd_power_off(struct generic_pm_domain *pd)
> > +{
> > +     struct genpd_power_state *state = &pd->states[pd->state_idx];
> > +     u32 *pd_state;
> > +
> > +     /* If we have failed to enable OSI mode, then abort power off. */
> > +     if (psci_has_osi_support() && !osi_mode_enabled)
> > +             return -EBUSY;
> > +
> > +     if (!state->data)
> > +             return 0;
> > +
> > +     /* When OSI mode is enabled, set the corresponding domain state. */
> > +     pd_state = state->data;
> > +     psci_set_domain_state(*pd_state);
>
> I trying to understand how would this scale to level 2(cluster of
> clusters or for simply system). The current code for psci_set_domain_state
> just stores the value @pd_state into per-cpu domain_state. E.g.: Now if
> the system level pd is getting called after cluster PD, it will set the
> domain state to system level PD state. It won't work with original
> format and it may work with extended format if it's carefully crafted.
> In short, the point is just over-writing domain_state is asking for
> troubles IMO.

Thanks for spotting this!

While walking upwards in the PM domain topology, I thought I was ORing
the domain states, but clearly the code isn't doing that.

In principle we need to do the below instead.

pd_state = state->data;
composite_pd_state = *pd_state | psci_get_domain_state();
psci_set_domain_state(composite_pd_state);

Kind regards
Uffe
Sudeep Holla July 18, 2019, 1:19 p.m. UTC | #6
On Thu, Jul 18, 2019 at 01:04:03PM +0200, Ulf Hansson wrote:
> On Tue, 16 Jul 2019 at 17:05, Sudeep Holla <sudeep.holla@arm.com> wrote:
> >
> > On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
> > > When the hierarchical CPU topology layout is used in DT, we need to setup
> > > the corresponding PM domain data structures, as to allow a CPU and a group
> > > of CPUs to be power managed accordingly. Let's enable this by deploying
> > > support through the genpd interface.
> > >
> > > Additionally, when the OS initiated mode is supported by the PSCI FW, let's
> > > also parse the domain idle states DT bindings as to make genpd responsible
> > > for the state selection, when the states are compatible with
> > > "domain-idle-state". Otherwise, when only Platform Coordinated mode is
> > > supported, we rely solely on the state selection to be managed through the
> > > regular cpuidle framework.
> > >
> > > If the initialization of the PM domain data structures succeeds and the OS
> > > initiated mode is supported, we try to switch to it. In case it fails,
> > > let's fall back into a degraded mode, rather than bailing out and returning
> > > an error code.
> > >
> > > Due to that the OS initiated mode may become enabled, we need to adjust to
> > > maintain backwards compatibility for a kernel started through a kexec call.
> > > Do this by explicitly switch to Platform Coordinated mode during boot.
> > >
> > > Finally, the actual initialization of the PM domain data structures, is
> > > done via calling the new shared function, psci_dt_init_pm_domains().
> > > However, this is implemented by subsequent changes.
> > >
> > > Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
> > > Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
> > > Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
> > > ---
> > >
> > > Changes:
> > >       - Simplify code setting domain_state at power off.
> > >       - Use the genpd ->free_state() callback to manage freeing of states.
> > >       - Fixup a bogus while loop.
> > >
> > > ---
> > >  drivers/firmware/psci/Makefile         |   2 +-
> > >  drivers/firmware/psci/psci.c           |   7 +-
> > >  drivers/firmware/psci/psci.h           |   5 +
> > >  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
> > >  4 files changed, 280 insertions(+), 2 deletions(-)
> > >  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
> > >
> >
> > [...]
> >
> > >  #endif /* __PSCI_H */
> > > diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
> > > new file mode 100644
> > > index 000000000000..3c6ca846caf4
> > > --- /dev/null
> > > +++ b/drivers/firmware/psci/psci_pm_domain.c
> > > @@ -0,0 +1,268 @@
> > > +// SPDX-License-Identifier: GPL-2.0
> > > +/*
> > > + * PM domains for CPUs via genpd - managed by PSCI.
> > > + *
> > > + * Copyright (C) 2019 Linaro Ltd.
> > > + * Author: Ulf Hansson <ulf.hansson@linaro.org>
> > > + *
> > > + */
> > > +
> >
> > [...]
> >
> > > +static int psci_pd_power_off(struct generic_pm_domain *pd)
> > > +{
> > > +     struct genpd_power_state *state = &pd->states[pd->state_idx];
> > > +     u32 *pd_state;
> > > +
> > > +     /* If we have failed to enable OSI mode, then abort power off. */
> > > +     if (psci_has_osi_support() && !osi_mode_enabled)
> > > +             return -EBUSY;
> > > +
> > > +     if (!state->data)
> > > +             return 0;
> > > +
> > > +     /* When OSI mode is enabled, set the corresponding domain state. */
> > > +     pd_state = state->data;
> > > +     psci_set_domain_state(*pd_state);
> >
> > I trying to understand how would this scale to level 2(cluster of
> > clusters or for simply system). The current code for psci_set_domain_state
> > just stores the value @pd_state into per-cpu domain_state. E.g.: Now if
> > the system level pd is getting called after cluster PD, it will set the
> > domain state to system level PD state. It won't work with original
> > format and it may work with extended format if it's carefully crafted.
> > In short, the point is just over-writing domain_state is asking for
> > troubles IMO.
>
> Thanks for spotting this!
>
> While walking upwards in the PM domain topology, I thought I was ORing
> the domain states, but clearly the code isn't doing that.
>
> In principle we need to do the below instead.
>
> pd_state = state->data;
> composite_pd_state = *pd_state | psci_get_domain_state();
> psci_set_domain_state(composite_pd_state);
>

Yes 2 different issues here:
1. The direct assignment overwriting the value is problem which you agree.
2. The OR logic on it's own is bit not so clear from the specification.
   Since firmware authors need to be aware of this to make all these
   work. So it's not implicit, either we set this requirement in form
   of binding. We were also thinking of stating composite state in the
   DT, still just a thought, need to come up with examples/illustrations.

--
Regards,
Sudeep
Lina Iyer July 18, 2019, 5:57 p.m. UTC | #7
On Thu, Jul 18 2019 at 07:19 -0600, Sudeep Holla wrote:
>On Thu, Jul 18, 2019 at 01:04:03PM +0200, Ulf Hansson wrote:
>> On Tue, 16 Jul 2019 at 17:05, Sudeep Holla <sudeep.holla@arm.com> wrote:
>> >
>> > On Mon, May 13, 2019 at 09:22:51PM +0200, Ulf Hansson wrote:
>> > > When the hierarchical CPU topology layout is used in DT, we need to setup
>> > > the corresponding PM domain data structures, as to allow a CPU and a group
>> > > of CPUs to be power managed accordingly. Let's enable this by deploying
>> > > support through the genpd interface.
>> > >
>> > > Additionally, when the OS initiated mode is supported by the PSCI FW, let's
>> > > also parse the domain idle states DT bindings as to make genpd responsible
>> > > for the state selection, when the states are compatible with
>> > > "domain-idle-state". Otherwise, when only Platform Coordinated mode is
>> > > supported, we rely solely on the state selection to be managed through the
>> > > regular cpuidle framework.
>> > >
>> > > If the initialization of the PM domain data structures succeeds and the OS
>> > > initiated mode is supported, we try to switch to it. In case it fails,
>> > > let's fall back into a degraded mode, rather than bailing out and returning
>> > > an error code.
>> > >
>> > > Due to that the OS initiated mode may become enabled, we need to adjust to
>> > > maintain backwards compatibility for a kernel started through a kexec call.
>> > > Do this by explicitly switch to Platform Coordinated mode during boot.
>> > >
>> > > Finally, the actual initialization of the PM domain data structures, is
>> > > done via calling the new shared function, psci_dt_init_pm_domains().
>> > > However, this is implemented by subsequent changes.
>> > >
>> > > Co-developed-by: Lina Iyer <lina.iyer@linaro.org>
>> > > Signed-off-by: Lina Iyer <lina.iyer@linaro.org>
>> > > Signed-off-by: Ulf Hansson <ulf.hansson@linaro.org>
>> > > ---
>> > >
>> > > Changes:
>> > >       - Simplify code setting domain_state at power off.
>> > >       - Use the genpd ->free_state() callback to manage freeing of states.
>> > >       - Fixup a bogus while loop.
>> > >
>> > > ---
>> > >  drivers/firmware/psci/Makefile         |   2 +-
>> > >  drivers/firmware/psci/psci.c           |   7 +-
>> > >  drivers/firmware/psci/psci.h           |   5 +
>> > >  drivers/firmware/psci/psci_pm_domain.c | 268 +++++++++++++++++++++++++
>> > >  4 files changed, 280 insertions(+), 2 deletions(-)
>> > >  create mode 100644 drivers/firmware/psci/psci_pm_domain.c
>> > >
>> >
>> > [...]
>> >
>> > >  #endif /* __PSCI_H */
>> > > diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
>> > > new file mode 100644
>> > > index 000000000000..3c6ca846caf4
>> > > --- /dev/null
>> > > +++ b/drivers/firmware/psci/psci_pm_domain.c
>> > > @@ -0,0 +1,268 @@
>> > > +// SPDX-License-Identifier: GPL-2.0
>> > > +/*
>> > > + * PM domains for CPUs via genpd - managed by PSCI.
>> > > + *
>> > > + * Copyright (C) 2019 Linaro Ltd.
>> > > + * Author: Ulf Hansson <ulf.hansson@linaro.org>
>> > > + *
>> > > + */
>> > > +
>> >
>> > [...]
>> >
>> > > +static int psci_pd_power_off(struct generic_pm_domain *pd)
>> > > +{
>> > > +     struct genpd_power_state *state = &pd->states[pd->state_idx];
>> > > +     u32 *pd_state;
>> > > +
>> > > +     /* If we have failed to enable OSI mode, then abort power off. */
>> > > +     if (psci_has_osi_support() && !osi_mode_enabled)
>> > > +             return -EBUSY;
>> > > +
>> > > +     if (!state->data)
>> > > +             return 0;
>> > > +
>> > > +     /* When OSI mode is enabled, set the corresponding domain state. */
>> > > +     pd_state = state->data;
>> > > +     psci_set_domain_state(*pd_state);
>> >
>> > I trying to understand how would this scale to level 2(cluster of
>> > clusters or for simply system). The current code for psci_set_domain_state
>> > just stores the value @pd_state into per-cpu domain_state. E.g.: Now if
>> > the system level pd is getting called after cluster PD, it will set the
>> > domain state to system level PD state. It won't work with original
>> > format and it may work with extended format if it's carefully crafted.
>> > In short, the point is just over-writing domain_state is asking for
>> > troubles IMO.
>>
>> Thanks for spotting this!
>>
>> While walking upwards in the PM domain topology, I thought I was ORing
>> the domain states, but clearly the code isn't doing that.
>>
>> In principle we need to do the below instead.
>>
>> pd_state = state->data;
>> composite_pd_state = *pd_state | psci_get_domain_state();
>> psci_set_domain_state(composite_pd_state);
>>
>
>Yes 2 different issues here:
>1. The direct assignment overwriting the value is problem which you agree.
>2. The OR logic on it's own is bit not so clear from the specification.
>   Since firmware authors need to be aware of this to make all these
>   work. So it's not implicit, either we set this requirement in form
>   of binding. We were also thinking of stating composite state in the
>   DT, still just a thought, need to come up with examples/illustrations.
>
It is generally very obvious to firmware authors to map hardware
definitions to specific bits in the state param. If a cluster component
has more than on/off state, more bits are assigned to the define the
idle states of the component.
Addition is also an option, but there are enough bits compared to the
hardware components that we have in each state, that it hasn't been a
problem.

--Lina
Sudeep Holla July 19, 2019, 9:45 a.m. UTC | #8
On Thu, Jul 18, 2019 at 11:57:46AM -0600, Lina Iyer wrote:
> On Thu, Jul 18 2019 at 07:19 -0600, Sudeep Holla wrote:
[...]

> >
> > Yes 2 different issues here:
> > 1. The direct assignment overwriting the value is problem which you agree.
> > 2. The OR logic on it's own is bit not so clear from the specification.
> >   Since firmware authors need to be aware of this to make all these
> >   work. So it's not implicit, either we set this requirement in form
> >   of binding. We were also thinking of stating composite state in the
> >   DT, still just a thought, need to come up with examples/illustrations.
> >
> It is generally very obvious to firmware authors to map hardware
> definitions to specific bits in the state param. If a cluster component
> has more than on/off state, more bits are assigned to the define the
> idle states of the component.

While I completely agree that generally the firmware authors tend to
assign a bit(s) to indicate a state, it not so evident from the PSCI
specification. My worry is someone shouldn't come up with sequential
numbering and expect this to work.

I am fine with OR, but we need to document it somewhere so that we
can point people so that the driver in the kernel is not expected
to work with any other schema of numbering StateID that violates
the assumption.

> Addition is also an option, but there are enough bits compared to the
> hardware components that we have in each state, that it hasn't been a
> problem.
>
Yes, but with extended format, the StateType move to bit 30, and if
we represent each individual state to indicate that bit correctly(I
except everyone to so that and it shouldn't cause issue if we OR the
parameters to get composite states), addition may cause issues.

--
Regards,
Sudeep

Patch
diff mbox series

diff --git a/drivers/firmware/psci/Makefile b/drivers/firmware/psci/Makefile
index 1956b882470f..ff300f1fec86 100644
--- a/drivers/firmware/psci/Makefile
+++ b/drivers/firmware/psci/Makefile
@@ -1,4 +1,4 @@ 
 # SPDX-License-Identifier: GPL-2.0
 #
-obj-$(CONFIG_ARM_PSCI_FW)	+= psci.o
+obj-$(CONFIG_ARM_PSCI_FW)	+= psci.o psci_pm_domain.o
 obj-$(CONFIG_ARM_PSCI_CHECKER)	+= psci_checker.o
diff --git a/drivers/firmware/psci/psci.c b/drivers/firmware/psci/psci.c
index 0e91d864e346..bfef300b7ebe 100644
--- a/drivers/firmware/psci/psci.c
+++ b/drivers/firmware/psci/psci.c
@@ -721,9 +721,14 @@  static int __init psci_1_0_init(struct device_node *np)
 	if (err)
 		return err;
 
-	if (psci_has_osi_support())
+	if (psci_has_osi_support()) {
 		pr_info("OSI mode supported.\n");
 
+		/* Make sure we default to PC mode. */
+		invoke_psci_fn(PSCI_1_0_FN_SET_SUSPEND_MODE,
+			       PSCI_1_0_SUSPEND_MODE_PC, 0, 0);
+	}
+
 	return 0;
 }
 
diff --git a/drivers/firmware/psci/psci.h b/drivers/firmware/psci/psci.h
index f2277c3ad405..00d2e3dcef49 100644
--- a/drivers/firmware/psci/psci.h
+++ b/drivers/firmware/psci/psci.h
@@ -11,6 +11,11 @@  bool psci_has_osi_support(void);
 #ifdef CONFIG_CPU_IDLE
 void psci_set_domain_state(u32 state);
 int psci_dt_parse_state_node(struct device_node *np, u32 *state);
+#ifdef CONFIG_PM_GENERIC_DOMAINS_OF
+int psci_dt_init_pm_domains(struct device_node *np);
+#else
+static inline int psci_dt_init_pm_domains(struct device_node *np) { return 0; }
+#endif
 #endif
 
 #endif /* __PSCI_H */
diff --git a/drivers/firmware/psci/psci_pm_domain.c b/drivers/firmware/psci/psci_pm_domain.c
new file mode 100644
index 000000000000..3c6ca846caf4
--- /dev/null
+++ b/drivers/firmware/psci/psci_pm_domain.c
@@ -0,0 +1,268 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * PM domains for CPUs via genpd - managed by PSCI.
+ *
+ * Copyright (C) 2019 Linaro Ltd.
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ */
+
+#define pr_fmt(fmt) "psci: " fmt
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/pm_domain.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+
+#include "psci.h"
+
+#if defined(CONFIG_CPU_IDLE) && defined(CONFIG_PM_GENERIC_DOMAINS_OF)
+
+struct psci_pd_provider {
+	struct list_head link;
+	struct device_node *node;
+};
+
+static LIST_HEAD(psci_pd_providers);
+static bool osi_mode_enabled;
+
+static int psci_pd_power_off(struct generic_pm_domain *pd)
+{
+	struct genpd_power_state *state = &pd->states[pd->state_idx];
+	u32 *pd_state;
+
+	/* If we have failed to enable OSI mode, then abort power off. */
+	if (psci_has_osi_support() && !osi_mode_enabled)
+		return -EBUSY;
+
+	if (!state->data)
+		return 0;
+
+	/* When OSI mode is enabled, set the corresponding domain state. */
+	pd_state = state->data;
+	psci_set_domain_state(*pd_state);
+
+	return 0;
+}
+
+static int psci_pd_parse_state_nodes(struct genpd_power_state *states,
+				int state_count)
+{
+	int i, ret;
+	u32 psci_state, *psci_state_buf;
+
+	for (i = 0; i < state_count; i++) {
+		ret = psci_dt_parse_state_node(to_of_node(states[i].fwnode),
+					&psci_state);
+		if (ret)
+			goto free_state;
+
+		psci_state_buf = kmalloc(sizeof(u32), GFP_KERNEL);
+		if (!psci_state_buf) {
+			ret = -ENOMEM;
+			goto free_state;
+		}
+		*psci_state_buf = psci_state;
+		states[i].data = psci_state_buf;
+	}
+
+	return 0;
+
+free_state:
+	i--;
+	for (; i >= 0; i--)
+		kfree(states[i].data);
+	return ret;
+}
+
+static int psci_pd_parse_states(struct device_node *np,
+			struct genpd_power_state **states, int *state_count)
+{
+	int ret;
+
+	/* Parse the domain idle states. */
+	ret = of_genpd_parse_idle_states(np, states, state_count);
+	if (ret)
+		return ret;
+
+	/* Fill out the PSCI specifics for each found state. */
+	ret = psci_pd_parse_state_nodes(*states, *state_count);
+	if (ret)
+		kfree(*states);
+
+	return ret;
+}
+
+static void psci_pd_free_states(struct genpd_power_state *states,
+				unsigned int state_count)
+{
+	int i;
+
+	for (i = 0; i < state_count; i++)
+		kfree(states[i].data);
+	kfree(states);
+}
+
+static int psci_pd_init(struct device_node *np)
+{
+	struct generic_pm_domain *pd;
+	struct psci_pd_provider *pd_provider;
+	struct dev_power_governor *pd_gov;
+	struct genpd_power_state *states = NULL;
+	int ret = -ENOMEM, state_count = 0;
+
+	pd = kzalloc(sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		goto out;
+
+	pd_provider = kzalloc(sizeof(*pd_provider), GFP_KERNEL);
+	if (!pd_provider)
+		goto free_pd;
+
+	pd->name = kasprintf(GFP_KERNEL, "%pOF", np);
+	if (!pd->name)
+		goto free_pd_prov;
+
+	/*
+	 * For OSI mode, parse the domain idle states and let genpd manage the
+	 * state selection for those being compatible with "domain-idle-state".
+	 */
+	if (psci_has_osi_support()) {
+		ret = psci_pd_parse_states(np, &states, &state_count);
+		if (ret)
+			goto free_name;
+		pd->free_states = psci_pd_free_states;
+	}
+
+	pd->name = kbasename(pd->name);
+	pd->power_off = psci_pd_power_off;
+	pd->states = states;
+	pd->state_count = state_count;
+	pd->flags |= GENPD_FLAG_IRQ_SAFE | GENPD_FLAG_CPU_DOMAIN;
+
+	/* Use governor for CPU PM domains if it has some states to manage. */
+	pd_gov = state_count > 0 ? &pm_domain_cpu_gov : NULL;
+
+	ret = pm_genpd_init(pd, pd_gov, false);
+	if (ret) {
+		psci_pd_free_states(states, state_count);
+		goto free_name;
+	}
+
+	ret = of_genpd_add_provider_simple(np, pd);
+	if (ret)
+		goto remove_pd;
+
+	pd_provider->node = of_node_get(np);
+	list_add(&pd_provider->link, &psci_pd_providers);
+
+	pr_debug("init PM domain %s\n", pd->name);
+	return 0;
+
+remove_pd:
+	pm_genpd_remove(pd);
+free_name:
+	kfree(pd->name);
+free_pd_prov:
+	kfree(pd_provider);
+free_pd:
+	kfree(pd);
+out:
+	pr_err("failed to init PM domain ret=%d %pOF\n", ret, np);
+	return ret;
+}
+
+static void psci_pd_remove(void)
+{
+	struct psci_pd_provider *pd_provider, *it;
+	struct generic_pm_domain *genpd;
+
+	list_for_each_entry_safe(pd_provider, it, &psci_pd_providers, link) {
+		of_genpd_del_provider(pd_provider->node);
+
+		genpd = of_genpd_remove_last(pd_provider->node);
+		if (!IS_ERR(genpd))
+			kfree(genpd);
+
+		of_node_put(pd_provider->node);
+		list_del(&pd_provider->link);
+		kfree(pd_provider);
+	}
+}
+
+static int psci_pd_init_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_add_subdomain(&parent, &child);
+		of_node_put(parent.np);
+		if (ret) {
+			of_node_put(node);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int psci_dt_init_pm_domains(struct device_node *np)
+{
+	struct device_node *node;
+	int ret, pd_count = 0;
+
+	/*
+	 * Parse child nodes for the "#power-domain-cells" property and
+	 * initialize a genpd/genpd-of-provider pair when it's found.
+	 */
+	for_each_child_of_node(np, node) {
+		if (!of_find_property(node, "#power-domain-cells", NULL))
+			continue;
+
+		ret = psci_pd_init(node);
+		if (ret)
+			goto put_node;
+
+		pd_count++;
+	}
+
+	/* Bail out if not using the hierarchical CPU topology. */
+	if (!pd_count)
+		return 0;
+
+	/* Link genpd masters/subdomains to model the CPU topology. */
+	ret = psci_pd_init_topology(np);
+	if (ret)
+		goto remove_pd;
+
+	/* Try to enable OSI mode if supported. */
+	if (psci_has_osi_support()) {
+		ret = psci_set_osi_mode();
+		if (ret)
+			pr_warn("failed to enable OSI mode: %d\n", ret);
+		else
+			osi_mode_enabled = true;
+	}
+
+	pr_info("Initialized CPU PM domain topology\n");
+	return pd_count;
+
+put_node:
+	of_node_put(node);
+remove_pd:
+	if (pd_count)
+		psci_pd_remove();
+	pr_err("failed to create CPU PM domains ret=%d\n", ret);
+	return ret;
+}
+#endif