diff mbox series

[RESEND,v4,1/3] cpuidle: qcom_spm: Detach state machine from main SPM handling

Message ID 20210618180907.258149-2-angelogioacchino.delregno@somainline.org (mailing list archive)
State Superseded
Headers show
Series Implement SPM/SAW for MSM8998 and SDM6xx | expand

Commit Message

AngeloGioacchino Del Regno June 18, 2021, 6:09 p.m. UTC
In commit a871be6b8eee ("cpuidle: Convert Qualcomm SPM driver to a generic
CPUidle driver") the SPM driver has been converted to a
generic CPUidle driver: that was mainly made to simplify the
driver and that was a great accomplishment;
Though, it was ignored that the SPM driver is not used only
on the ARM architecture.

In preparation for the enablement of SPM features on AArch64/ARM64,
split the cpuidle-qcom-spm driver in two: the CPUIdle related
state machine (currently used only on ARM SoCs) stays there, while
the SPM communication handling lands back in soc/qcom/spm.c and
also making sure to not discard the simplifications that were
introduced in the aforementioned commit.

Since now the "two drivers" are split, the SCM dependency in the
main SPM handling is gone and for this reason it was also possible
to move the SPM initialization early: this will also make sure that
whenever the SAW CPUIdle driver is getting initialized, the SPM
driver will be ready to do the job.

Please note that the anticipation of the SPM initialization was
also done to optimize the boot times on platforms that have their
CPU/L2 idle states managed by other means (such as PSCI), while
needing SAW initialization for other purposes, like AVS control.

Signed-off-by: AngeloGioacchino Del Regno <angelogioacchino.delregno@somainline.org>
---
 drivers/cpuidle/Kconfig.arm        |   1 +
 drivers/cpuidle/cpuidle-qcom-spm.c | 294 ++++++-----------------------
 drivers/soc/qcom/Kconfig           |   9 +
 drivers/soc/qcom/Makefile          |   1 +
 drivers/soc/qcom/spm.c             | 198 +++++++++++++++++++
 include/soc/qcom/spm.h             |  45 +++++
 6 files changed, 312 insertions(+), 236 deletions(-)
 create mode 100644 drivers/soc/qcom/spm.c
 create mode 100644 include/soc/qcom/spm.h

Comments

Stephan Gerhold June 18, 2021, 9:37 p.m. UTC | #1
Hi,

Thanks for the patch!

On Fri, Jun 18, 2021 at 08:09:05PM +0200, AngeloGioacchino Del Regno wrote:
> In commit a871be6b8eee ("cpuidle: Convert Qualcomm SPM driver to a generic
> CPUidle driver") the SPM driver has been converted to a
> generic CPUidle driver: that was mainly made to simplify the
> driver and that was a great accomplishment;
>

Hmm, you mention my commit but did not Cc me.
Only saw this patch while randomly looking through linux-arm-msm. :)

> Though, it was ignored that the SPM driver is not used only
> on the ARM architecture.

This sentence is a bit misleading IMO. In mainline the SPM driver is
*currently* only used for CPUidle, and the old driver was as
CPUidle-specific as the new one. So saying that I "ignored" something
here is kind of wrong. :)

Can you re-phrase this a bit to say that the SPM hardware is also
used for power-collapse of the CPU caches/AVS/whatever and therefore we
need to refactor the driver to something more independent?

> In preparation for the enablement of SPM features on AArch64/ARM64,
> split the cpuidle-qcom-spm driver in two: the CPUIdle related
> state machine (currently used only on ARM SoCs) stays there, while
> the SPM communication handling lands back in soc/qcom/spm.c and
> also making sure to not discard the simplifications that were
> introduced in the aforementioned commit.
> 
> Since now the "two drivers" are split, the SCM dependency in the
> main SPM handling is gone and for this reason it was also possible
> to move the SPM initialization early: this will also make sure that
> whenever the SAW CPUIdle driver is getting initialized, the SPM
> driver will be ready to do the job.
> 
> Please note that the anticipation of the SPM initialization was
> also done to optimize the boot times on platforms that have their
> CPU/L2 idle states managed by other means (such as PSCI), while
> needing SAW initialization for other purposes, like AVS control.
> 
> Signed-off-by: AngeloGioacchino Del Regno <angelogioacchino.delregno@somainline.org>

The diff is quite hard to read for me (just the nature of this change,
not your fault). Just mentioning this in case I miss something obvious.

> ---
>  drivers/cpuidle/Kconfig.arm        |   1 +
>  drivers/cpuidle/cpuidle-qcom-spm.c | 294 ++++++-----------------------
>  drivers/soc/qcom/Kconfig           |   9 +
>  drivers/soc/qcom/Makefile          |   1 +
>  drivers/soc/qcom/spm.c             | 198 +++++++++++++++++++
>  include/soc/qcom/spm.h             |  45 +++++
>  6 files changed, 312 insertions(+), 236 deletions(-)
>  create mode 100644 drivers/soc/qcom/spm.c
>  create mode 100644 include/soc/qcom/spm.h
> 
> [...]
> diff --git a/drivers/cpuidle/cpuidle-qcom-spm.c b/drivers/cpuidle/cpuidle-qcom-spm.c
> index adf91a6e4d7d..3dd7bb10b82d 100644
> --- a/drivers/cpuidle/cpuidle-qcom-spm.c
> +++ b/drivers/cpuidle/cpuidle-qcom-spm.c
> [...]
> -static int spm_dev_probe(struct platform_device *pdev)
> +static int spm_cpuidle_drv_probe(struct platform_device *pdev)
>  {
>  	[...]
> +	for_each_possible_cpu(cpu) {
> +		ret = spm_cpuidle_register(cpu);
> +		if (ret != -ENODEV) {
> +			dev_err(&pdev->dev,
> +				"Cannot register for CPU%d: %d\n", cpu, ret);
> +			break;

Huh, why does this error for ret = 0? :S

[    0.736332] qcom-spm-cpuidle qcom-spm-cpuidle: Cannot register for CPU0: 0

A couple more lines in the UART log, then the device hangs forever.
For testing I changed this to: if (ret && ret != -ENODEV).

> [...]
> @@ -213,132 +80,87 @@ static const struct of_device_id qcom_idle_state_match[] = {
> -static int spm_cpuidle_init(struct cpuidle_driver *drv, int cpu)
> +static int spm_cpuidle_register(int cpu)
>  {
> +	struct platform_device *pdev = NULL;
> +	struct spm_driver_data *spm = NULL;
> +	struct device_node *cpu_node, *saw_node;
>  	int ret;
>  
> -	memcpy(drv, &qcom_spm_idle_driver, sizeof(*drv));
> -	drv->cpumask = (struct cpumask *)cpumask_of(cpu);

Somehow this line got lost, which means the first cpuidle_driver will
cover all CPUs and we will always fail to register the cpuidle_driver
for all other CPUs:

[    0.736591] failed to register cpuidle driver
[    0.744186] qcom-spm-cpuidle qcom-spm-cpuidle: Cannot register for CPU1: -16
[    0.748443] qcom-spm-cpuidle: probe of qcom-spm-cpuidle failed with error -16

(Then the device hangs forever.)

I added
spm->cpuidle_driver.cpumask = (struct cpumask *)cpumask_of(cpu);
below

> +	spm->cpuidle_driver = qcom_spm_idle_driver;

and this seems to make it boot again at least.

However, it seems a bit pointless now to have a separate cpuidle_driver
per CPU, since they are all registered at the same time. With my
refactoring this was kind of convenient because the SPM platform devices
could happily probe independently and just register a cpuidle_driver for
the CPU they belong to.

With your patch, the cpuidle_drivers are registered at the same time for
all CPUs, so we might as well use a single cpuidle_driver that covers
all CPUs (like you already do without setting cpumask).

Note that if you have a single cpuidle_driver for all CPUs you need to
refactor spm_enter_idle_state() a bit. The container_of() will no longer
work to get the CPU-specific SPM. Before my changes there was a
DEFINE_PER_CPU for this. I guess we need to bring that back.

>  	[...]
> +	ret = dt_init_idle_driver(&spm->cpuidle_driver,
> +				  qcom_idle_state_match, 1);
> +	if (ret <= 0)
> +		return ret ? : -ENODEV;
>  
> -	return drv;
> -}
> +	ret = qcom_scm_set_warm_boot_addr(cpu_resume_arm, cpumask_of(cpu));
> +	if (ret)
> +		return ret;

And the advantage here is that we should be able to do this with a
single firmware call (set the warm boot addr for all CPUs at once).

> [...]
> diff --git a/include/soc/qcom/spm.h b/include/soc/qcom/spm.h
> new file mode 100644
> index 000000000000..604eca2c4d4a
> --- /dev/null
> +++ b/include/soc/qcom/spm.h
> @@ -0,0 +1,45 @@
> +/* SPDX-License-Identifier: GPL-2.0-only */
> +/*
> + * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
> + * Copyright (c) 2014,2015, Linaro Ltd.
> + * Copyright (C) 2020, AngeloGioacchino Del Regno <kholk11@gmail.com>
> + */
> +
> +#ifndef __SPM_H__
> +#define __SPM_H__
> +
> +#include <linux/cpuidle.h>
> +
> +#define MAX_PMIC_DATA		2
> +#define MAX_SEQ_DATA		64
> +
> +enum pm_sleep_mode {
> +	PM_SLEEP_MODE_STBY,
> +	PM_SLEEP_MODE_RET,
> +	PM_SLEEP_MODE_SPC,
> +	PM_SLEEP_MODE_PC,
> +	PM_SLEEP_MODE_NR,
> +};
> +
> +struct spm_reg_data {
> +	const u16 *reg_offset;
> +	u32 spm_cfg;
> +	u32 spm_dly;
> +	u32 pmic_dly;
> +	u32 pmic_data[MAX_PMIC_DATA];
> +	u32 avs_ctl;
> +	u32 avs_limit;

Looks like you accidentally included changes from PATCH 2/3
("soc: qcom: spm: Implement support for SAWv4.1, SDM630/660 L2 AVS")
here, reg_offset u8 -> u16 and adding avs_ctl and avs_limit should be in
a separate patch. It's really hard to see that you added those here
while moving the code. :/

Thanks,
Stephan
AngeloGioacchino Del Regno June 18, 2021, 10:32 p.m. UTC | #2
Il 18/06/21 23:37, Stephan Gerhold ha scritto:
> Hi,
> 
> Thanks for the patch!
> 
> On Fri, Jun 18, 2021 at 08:09:05PM +0200, AngeloGioacchino Del Regno wrote:
>> In commit a871be6b8eee ("cpuidle: Convert Qualcomm SPM driver to a generic
>> CPUidle driver") the SPM driver has been converted to a
>> generic CPUidle driver: that was mainly made to simplify the
>> driver and that was a great accomplishment;
>>
> 
> Hmm, you mention my commit but did not Cc me.
> Only saw this patch while randomly looking through linux-arm-msm. :)
> 

Sorry, I've originally sent this series 6 months ago, things may fly
off of my head... :))

>> Though, it was ignored that the SPM driver is not used only
>> on the ARM architecture.
> 
> This sentence is a bit misleading IMO. In mainline the SPM driver is
> *currently* only used for CPUidle, and the old driver was as
> CPUidle-specific as the new one. So saying that I "ignored" something
> here is kind of wrong. :)
> 
> Can you re-phrase this a bit to say that the SPM hardware is also
> used for power-collapse of the CPU caches/AVS/whatever and therefore we
> need to refactor the driver to something more independent?
> 

On SAWv4.1, the SPM is used to regulate AVS limits but *not* power
collapse of the CPU caches "and whatever": platforms with this version
are using different HW to accomplish that.

>> In preparation for the enablement of SPM features on AArch64/ARM64,
>> split the cpuidle-qcom-spm driver in two: the CPUIdle related
>> state machine (currently used only on ARM SoCs) stays there, while
>> the SPM communication handling lands back in soc/qcom/spm.c and
>> also making sure to not discard the simplifications that were
>> introduced in the aforementioned commit.
>>
>> Since now the "two drivers" are split, the SCM dependency in the
>> main SPM handling is gone and for this reason it was also possible
>> to move the SPM initialization early: this will also make sure that
>> whenever the SAW CPUIdle driver is getting initialized, the SPM
>> driver will be ready to do the job.
>>
>> Please note that the anticipation of the SPM initialization was
>> also done to optimize the boot times on platforms that have their
>> CPU/L2 idle states managed by other means (such as PSCI), while
>> needing SAW initialization for other purposes, like AVS control.
>>
>> Signed-off-by: AngeloGioacchino Del Regno <angelogioacchino.delregno@somainline.org>
> 
> The diff is quite hard to read for me (just the nature of this change,
> not your fault). Just mentioning this in case I miss something obvious.
> 
>> ---
>>   drivers/cpuidle/Kconfig.arm        |   1 +
>>   drivers/cpuidle/cpuidle-qcom-spm.c | 294 ++++++-----------------------
>>   drivers/soc/qcom/Kconfig           |   9 +
>>   drivers/soc/qcom/Makefile          |   1 +
>>   drivers/soc/qcom/spm.c             | 198 +++++++++++++++++++
>>   include/soc/qcom/spm.h             |  45 +++++
>>   6 files changed, 312 insertions(+), 236 deletions(-)
>>   create mode 100644 drivers/soc/qcom/spm.c
>>   create mode 100644 include/soc/qcom/spm.h
>>
>> [...]
>> diff --git a/drivers/cpuidle/cpuidle-qcom-spm.c b/drivers/cpuidle/cpuidle-qcom-spm.c
>> index adf91a6e4d7d..3dd7bb10b82d 100644
>> --- a/drivers/cpuidle/cpuidle-qcom-spm.c
>> +++ b/drivers/cpuidle/cpuidle-qcom-spm.c
>> [...]
>> -static int spm_dev_probe(struct platform_device *pdev)
>> +static int spm_cpuidle_drv_probe(struct platform_device *pdev)
>>   {
>>   	[...]
>> +	for_each_possible_cpu(cpu) {
>> +		ret = spm_cpuidle_register(cpu);
>> +		if (ret != -ENODEV) {
>> +			dev_err(&pdev->dev,
>> +				"Cannot register for CPU%d: %d\n", cpu, ret);
>> +			break;
> 
> Huh, why does this error for ret = 0? :S
> 
> [    0.736332] qcom-spm-cpuidle qcom-spm-cpuidle: Cannot register for CPU0: 0
> 
> A couple more lines in the UART log, then the device hangs forever.
> For testing I changed this to: if (ret && ret != -ENODEV).
> 

I have no idea what happened here; this was indeed supposed to be what
you just pointed out.

>> [...]
>> @@ -213,132 +80,87 @@ static const struct of_device_id qcom_idle_state_match[] = {
>> -static int spm_cpuidle_init(struct cpuidle_driver *drv, int cpu)
>> +static int spm_cpuidle_register(int cpu)
>>   {
>> +	struct platform_device *pdev = NULL;
>> +	struct spm_driver_data *spm = NULL;
>> +	struct device_node *cpu_node, *saw_node;
>>   	int ret;
>>   
>> -	memcpy(drv, &qcom_spm_idle_driver, sizeof(*drv));
>> -	drv->cpumask = (struct cpumask *)cpumask_of(cpu);
> 
> Somehow this line got lost, which means the first cpuidle_driver will
> cover all CPUs and we will always fail to register the cpuidle_driver
> for all other CPUs:
> 
> [    0.736591] failed to register cpuidle driver
> [    0.744186] qcom-spm-cpuidle qcom-spm-cpuidle: Cannot register for CPU1: -16
> [    0.748443] qcom-spm-cpuidle: probe of qcom-spm-cpuidle failed with error -16
> 
> (Then the device hangs forever.)
> 

So you have discovered a bug for which your platform dies when SPM
does not probe, probably due to something else being dependant on this.
In this case, I would encourage you to produce a fix for your platform
to not unexpectedly just hang forever if *some driver* doesn't probe:
that's definitely not right.

> I added
> spm->cpuidle_driver.cpumask = (struct cpumask *)cpumask_of(cpu);
> below
> 
>> +	spm->cpuidle_driver = qcom_spm_idle_driver;
> 
> and this seems to make it boot again at least.
> 

I really think that I've originally messed up the patch originally:
this doesn't seem to be the right version, even though it was marked as
v4. I trusted my folders organization too much. Apologies.

> However, it seems a bit pointless now to have a separate cpuidle_driver
> per CPU, since they are all registered at the same time. With my
> refactoring this was kind of convenient because the SPM platform devices
> could happily probe independently and just register a cpuidle_driver for
> the CPU they belong to.
> 
> With your patch, the cpuidle_drivers are registered at the same time for
> all CPUs, so we might as well use a single cpuidle_driver that covers
> all CPUs (like you already do without setting cpumask).
> 
> Note that if you have a single cpuidle_driver for all CPUs you need to
> refactor spm_enter_idle_state() a bit. The container_of() will no longer
> work to get the CPU-specific SPM. Before my changes there was a
> DEFINE_PER_CPU for this. I guess we need to bring that back.
> 
>>   	[...]
>> +	ret = dt_init_idle_driver(&spm->cpuidle_driver,
>> +				  qcom_idle_state_match, 1);
>> +	if (ret <= 0)
>> +		return ret ? : -ENODEV;
>>   
>> -	return drv;
>> -}
>> +	ret = qcom_scm_set_warm_boot_addr(cpu_resume_arm, cpumask_of(cpu));
>> +	if (ret)
>> +		return ret;
> 
> And the advantage here is that we should be able to do this with a
> single firmware call (set the warm boot addr for all CPUs at once).
> 


Probably staying with setting the cpumask is a better option; we don't

really know if there's any platform requiring that kind of quirk: at
least downstream, I recall that they made sure to send multiple calls.


>> [...]
>> diff --git a/include/soc/qcom/spm.h b/include/soc/qcom/spm.h
>> new file mode 100644
>> index 000000000000..604eca2c4d4a
>> --- /dev/null
>> +++ b/include/soc/qcom/spm.h
>> @@ -0,0 +1,45 @@
>> +/* SPDX-License-Identifier: GPL-2.0-only */
>> +/*
>> + * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
>> + * Copyright (c) 2014,2015, Linaro Ltd.
>> + * Copyright (C) 2020, AngeloGioacchino Del Regno <kholk11@gmail.com>
>> + */
>> +
>> +#ifndef __SPM_H__
>> +#define __SPM_H__
>> +
>> +#include <linux/cpuidle.h>
>> +
>> +#define MAX_PMIC_DATA		2
>> +#define MAX_SEQ_DATA		64
>> +
>> +enum pm_sleep_mode {
>> +	PM_SLEEP_MODE_STBY,
>> +	PM_SLEEP_MODE_RET,
>> +	PM_SLEEP_MODE_SPC,
>> +	PM_SLEEP_MODE_PC,
>> +	PM_SLEEP_MODE_NR,
>> +};
>> +
>> +struct spm_reg_data {
>> +	const u16 *reg_offset;
>> +	u32 spm_cfg;
>> +	u32 spm_dly;
>> +	u32 pmic_dly;
>> +	u32 pmic_data[MAX_PMIC_DATA];
>> +	u32 avs_ctl;
>> +	u32 avs_limit;
> 
> Looks like you accidentally included changes from PATCH 2/3
> ("soc: qcom: spm: Implement support for SAWv4.1, SDM630/660 L2 AVS")
> here, reg_offset u8 -> u16 and adding avs_ctl and avs_limit should be in
> a separate patch. It's really hard to see that you added those here
> while moving the code. :/
> 

This change belongs to 2/3.
Will send a v5 with fixes.

> Thanks,
> Stephan
>
Stephan Gerhold June 18, 2021, 10:45 p.m. UTC | #3
On Sat, Jun 19, 2021 at 12:32:50AM +0200, AngeloGioacchino Del Regno wrote:
> Il 18/06/21 23:37, Stephan Gerhold ha scritto:
> > > Though, it was ignored that the SPM driver is not used only
> > > on the ARM architecture.
> > 
> > This sentence is a bit misleading IMO. In mainline the SPM driver is
> > *currently* only used for CPUidle, and the old driver was as
> > CPUidle-specific as the new one. So saying that I "ignored" something
> > here is kind of wrong. :)
> > 
> > Can you re-phrase this a bit to say that the SPM hardware is also
> > used for power-collapse of the CPU caches/AVS/whatever and therefore we
> > need to refactor the driver to something more independent?
> > 
> 
> On SAWv4.1, the SPM is used to regulate AVS limits but *not* power
> collapse of the CPU caches "and whatever": platforms with this version
> are using different HW to accomplish that.
> 

OK, thanks for clarifying! My point stands though, I don't think
I "ignored" anything in my refactoring commit. :)

> 
> > > [...]
> > > @@ -213,132 +80,87 @@ static const struct of_device_id qcom_idle_state_match[] = {
> > > -static int spm_cpuidle_init(struct cpuidle_driver *drv, int cpu)
> > > +static int spm_cpuidle_register(int cpu)
> > >   {
> > > +	struct platform_device *pdev = NULL;
> > > +	struct spm_driver_data *spm = NULL;
> > > +	struct device_node *cpu_node, *saw_node;
> > >   	int ret;
> > > -	memcpy(drv, &qcom_spm_idle_driver, sizeof(*drv));
> > > -	drv->cpumask = (struct cpumask *)cpumask_of(cpu);
> > 
> > Somehow this line got lost, which means the first cpuidle_driver will
> > cover all CPUs and we will always fail to register the cpuidle_driver
> > for all other CPUs:
> > 
> > [    0.736591] failed to register cpuidle driver
> > [    0.744186] qcom-spm-cpuidle qcom-spm-cpuidle: Cannot register for CPU1: -16
> > [    0.748443] qcom-spm-cpuidle: probe of qcom-spm-cpuidle failed with error -16
> > 
> > (Then the device hangs forever.)
> > 
> 
> So you have discovered a bug for which your platform dies when SPM
> does not probe, probably due to something else being dependant on this.
> In this case, I would encourage you to produce a fix for your platform
> to not unexpectedly just hang forever if *some driver* doesn't probe:
> that's definitely not right.
> 

Fair enough, might investigate this further.

> > I added
> > spm->cpuidle_driver.cpumask = (struct cpumask *)cpumask_of(cpu);
> > below
> > 
> > > +	spm->cpuidle_driver = qcom_spm_idle_driver;
> > 
> > and this seems to make it boot again at least.
> > 
> 
> I really think that I've originally messed up the patch originally:
> this doesn't seem to be the right version, even though it was marked as
> v4. I trusted my folders organization too much. Apologies.
> 

It seems mostly equivalent with the previous v4 and v3, but didn't check
very carefully. No problem though, as long we can get it fixed. :)

> > However, it seems a bit pointless now to have a separate cpuidle_driver
> > per CPU, since they are all registered at the same time. With my
> > refactoring this was kind of convenient because the SPM platform devices
> > could happily probe independently and just register a cpuidle_driver for
> > the CPU they belong to.
> > 
> > With your patch, the cpuidle_drivers are registered at the same time for
> > all CPUs, so we might as well use a single cpuidle_driver that covers
> > all CPUs (like you already do without setting cpumask).
> > 
> > Note that if you have a single cpuidle_driver for all CPUs you need to
> > refactor spm_enter_idle_state() a bit. The container_of() will no longer
> > work to get the CPU-specific SPM. Before my changes there was a
> > DEFINE_PER_CPU for this. I guess we need to bring that back.
> > 
> > >   	[...]
> > > +	ret = dt_init_idle_driver(&spm->cpuidle_driver,
> > > +				  qcom_idle_state_match, 1);
> > > +	if (ret <= 0)
> > > +		return ret ? : -ENODEV;
> > > -	return drv;
> > > -}
> > > +	ret = qcom_scm_set_warm_boot_addr(cpu_resume_arm, cpumask_of(cpu));
> > > +	if (ret)
> > > +		return ret;
> > 
> > And the advantage here is that we should be able to do this with a
> > single firmware call (set the warm boot addr for all CPUs at once).
> > 
> 
> Probably staying with setting the cpumask is a better option; we don't
> really know if there's any platform requiring that kind of quirk: at
> least downstream, I recall that they made sure to send multiple calls.
> 

I don't think it really makes a difference but fair enough. I still
think it could be worth having a single cpuidle_driver (even if we call
qcom_scm_set_warm_boot_addr() separately for each CPU), but I suppose
this is more material for a potential follow-up patch.

> 
> > > [...]
> > > diff --git a/include/soc/qcom/spm.h b/include/soc/qcom/spm.h
> > > new file mode 100644
> > > index 000000000000..604eca2c4d4a
> > > --- /dev/null
> > > +++ b/include/soc/qcom/spm.h
> > > @@ -0,0 +1,45 @@
> > > +/* SPDX-License-Identifier: GPL-2.0-only */
> > > +/*
> > > + * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
> > > + * Copyright (c) 2014,2015, Linaro Ltd.
> > > + * Copyright (C) 2020, AngeloGioacchino Del Regno <kholk11@gmail.com>
> > > + */
> > > +
> > > +#ifndef __SPM_H__
> > > +#define __SPM_H__
> > > +
> > > +#include <linux/cpuidle.h>
> > > +
> > > +#define MAX_PMIC_DATA		2
> > > +#define MAX_SEQ_DATA		64
> > > +
> > > +enum pm_sleep_mode {
> > > +	PM_SLEEP_MODE_STBY,
> > > +	PM_SLEEP_MODE_RET,
> > > +	PM_SLEEP_MODE_SPC,
> > > +	PM_SLEEP_MODE_PC,
> > > +	PM_SLEEP_MODE_NR,
> > > +};
> > > +
> > > +struct spm_reg_data {
> > > +	const u16 *reg_offset;
> > > +	u32 spm_cfg;
> > > +	u32 spm_dly;
> > > +	u32 pmic_dly;
> > > +	u32 pmic_data[MAX_PMIC_DATA];
> > > +	u32 avs_ctl;
> > > +	u32 avs_limit;
> > 
> > Looks like you accidentally included changes from PATCH 2/3
> > ("soc: qcom: spm: Implement support for SAWv4.1, SDM630/660 L2 AVS")
> > here, reg_offset u8 -> u16 and adding avs_ctl and avs_limit should be in
> > a separate patch. It's really hard to see that you added those here
> > while moving the code. :/
> > 
> 
> This change belongs to 2/3.
> Will send a v5 with fixes.
> 

Thanks!
Stephan
diff mbox series

Patch

diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 0844fadc4be8..c5e2f4d0df04 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -112,6 +112,7 @@  config ARM_QCOM_SPM_CPUIDLE
 	select CPU_IDLE_MULTIPLE_DRIVERS
 	select DT_IDLE_STATES
 	select QCOM_SCM
+	select QCOM_SPM
 	help
 	  Select this to enable cpuidle for Qualcomm processors.
 	  The Subsystem Power Manager (SPM) controls low power modes for the
diff --git a/drivers/cpuidle/cpuidle-qcom-spm.c b/drivers/cpuidle/cpuidle-qcom-spm.c
index adf91a6e4d7d..3dd7bb10b82d 100644
--- a/drivers/cpuidle/cpuidle-qcom-spm.c
+++ b/drivers/cpuidle/cpuidle-qcom-spm.c
@@ -18,146 +18,13 @@ 
 #include <linux/cpuidle.h>
 #include <linux/cpu_pm.h>
 #include <linux/qcom_scm.h>
+#include <soc/qcom/spm.h>
 
 #include <asm/proc-fns.h>
 #include <asm/suspend.h>
 
 #include "dt_idle_states.h"
 
-#define MAX_PMIC_DATA		2
-#define MAX_SEQ_DATA		64
-#define SPM_CTL_INDEX		0x7f
-#define SPM_CTL_INDEX_SHIFT	4
-#define SPM_CTL_EN		BIT(0)
-
-enum pm_sleep_mode {
-	PM_SLEEP_MODE_STBY,
-	PM_SLEEP_MODE_RET,
-	PM_SLEEP_MODE_SPC,
-	PM_SLEEP_MODE_PC,
-	PM_SLEEP_MODE_NR,
-};
-
-enum spm_reg {
-	SPM_REG_CFG,
-	SPM_REG_SPM_CTL,
-	SPM_REG_DLY,
-	SPM_REG_PMIC_DLY,
-	SPM_REG_PMIC_DATA_0,
-	SPM_REG_PMIC_DATA_1,
-	SPM_REG_VCTL,
-	SPM_REG_SEQ_ENTRY,
-	SPM_REG_SPM_STS,
-	SPM_REG_PMIC_STS,
-	SPM_REG_NR,
-};
-
-struct spm_reg_data {
-	const u8 *reg_offset;
-	u32 spm_cfg;
-	u32 spm_dly;
-	u32 pmic_dly;
-	u32 pmic_data[MAX_PMIC_DATA];
-	u8 seq[MAX_SEQ_DATA];
-	u8 start_index[PM_SLEEP_MODE_NR];
-};
-
-struct spm_driver_data {
-	struct cpuidle_driver cpuidle_driver;
-	void __iomem *reg_base;
-	const struct spm_reg_data *reg_data;
-};
-
-static const u8 spm_reg_offset_v2_1[SPM_REG_NR] = {
-	[SPM_REG_CFG]		= 0x08,
-	[SPM_REG_SPM_CTL]	= 0x30,
-	[SPM_REG_DLY]		= 0x34,
-	[SPM_REG_SEQ_ENTRY]	= 0x80,
-};
-
-/* SPM register data for 8974, 8084 */
-static const struct spm_reg_data spm_reg_8974_8084_cpu  = {
-	.reg_offset = spm_reg_offset_v2_1,
-	.spm_cfg = 0x1,
-	.spm_dly = 0x3C102800,
-	.seq = { 0x03, 0x0B, 0x0F, 0x00, 0x20, 0x80, 0x10, 0xE8, 0x5B, 0x03,
-		0x3B, 0xE8, 0x5B, 0x82, 0x10, 0x0B, 0x30, 0x06, 0x26, 0x30,
-		0x0F },
-	.start_index[PM_SLEEP_MODE_STBY] = 0,
-	.start_index[PM_SLEEP_MODE_SPC] = 3,
-};
-
-static const u8 spm_reg_offset_v1_1[SPM_REG_NR] = {
-	[SPM_REG_CFG]		= 0x08,
-	[SPM_REG_SPM_CTL]	= 0x20,
-	[SPM_REG_PMIC_DLY]	= 0x24,
-	[SPM_REG_PMIC_DATA_0]	= 0x28,
-	[SPM_REG_PMIC_DATA_1]	= 0x2C,
-	[SPM_REG_SEQ_ENTRY]	= 0x80,
-};
-
-/* SPM register data for 8064 */
-static const struct spm_reg_data spm_reg_8064_cpu = {
-	.reg_offset = spm_reg_offset_v1_1,
-	.spm_cfg = 0x1F,
-	.pmic_dly = 0x02020004,
-	.pmic_data[0] = 0x0084009C,
-	.pmic_data[1] = 0x00A4001C,
-	.seq = { 0x03, 0x0F, 0x00, 0x24, 0x54, 0x10, 0x09, 0x03, 0x01,
-		0x10, 0x54, 0x30, 0x0C, 0x24, 0x30, 0x0F },
-	.start_index[PM_SLEEP_MODE_STBY] = 0,
-	.start_index[PM_SLEEP_MODE_SPC] = 2,
-};
-
-static inline void spm_register_write(struct spm_driver_data *drv,
-					enum spm_reg reg, u32 val)
-{
-	if (drv->reg_data->reg_offset[reg])
-		writel_relaxed(val, drv->reg_base +
-				drv->reg_data->reg_offset[reg]);
-}
-
-/* Ensure a guaranteed write, before return */
-static inline void spm_register_write_sync(struct spm_driver_data *drv,
-					enum spm_reg reg, u32 val)
-{
-	u32 ret;
-
-	if (!drv->reg_data->reg_offset[reg])
-		return;
-
-	do {
-		writel_relaxed(val, drv->reg_base +
-				drv->reg_data->reg_offset[reg]);
-		ret = readl_relaxed(drv->reg_base +
-				drv->reg_data->reg_offset[reg]);
-		if (ret == val)
-			break;
-		cpu_relax();
-	} while (1);
-}
-
-static inline u32 spm_register_read(struct spm_driver_data *drv,
-					enum spm_reg reg)
-{
-	return readl_relaxed(drv->reg_base + drv->reg_data->reg_offset[reg]);
-}
-
-static void spm_set_low_power_mode(struct spm_driver_data *drv,
-					enum pm_sleep_mode mode)
-{
-	u32 start_index;
-	u32 ctl_val;
-
-	start_index = drv->reg_data->start_index[mode];
-
-	ctl_val = spm_register_read(drv, SPM_REG_SPM_CTL);
-	ctl_val &= ~(SPM_CTL_INDEX << SPM_CTL_INDEX_SHIFT);
-	ctl_val |= start_index << SPM_CTL_INDEX_SHIFT;
-	ctl_val |= SPM_CTL_EN;
-	spm_register_write_sync(drv, SPM_REG_SPM_CTL, ctl_val);
-}
-
 static int qcom_pm_collapse(unsigned long int unused)
 {
 	qcom_scm_cpu_power_down(QCOM_SCM_CPU_PWR_DOWN_L2_ON);
@@ -213,132 +80,87 @@  static const struct of_device_id qcom_idle_state_match[] = {
 	{ },
 };
 
-static int spm_cpuidle_init(struct cpuidle_driver *drv, int cpu)
+static int spm_cpuidle_register(int cpu)
 {
+	struct platform_device *pdev = NULL;
+	struct spm_driver_data *spm = NULL;
+	struct device_node *cpu_node, *saw_node;
 	int ret;
 
-	memcpy(drv, &qcom_spm_idle_driver, sizeof(*drv));
-	drv->cpumask = (struct cpumask *)cpumask_of(cpu);
+	cpu_node = of_cpu_device_node_get(cpu);
+	if (!cpu_node)
+		return -ENODEV;
 
-	/* Parse idle states from device tree */
-	ret = dt_init_idle_driver(drv, qcom_idle_state_match, 1);
-	if (ret <= 0)
-		return ret ? : -ENODEV;
+	saw_node = of_parse_phandle(cpu_node, "qcom,saw", 0);
+	if (!saw_node)
+		return -ENODEV;
 
-	/* We have atleast one power down mode */
-	return qcom_scm_set_warm_boot_addr(cpu_resume_arm, drv->cpumask);
-}
+	pdev = of_find_device_by_node(saw_node);
+	of_node_put(saw_node);
+	of_node_put(cpu_node);
+	if (!pdev)
+		return -ENODEV;
 
-static struct spm_driver_data *spm_get_drv(struct platform_device *pdev,
-		int *spm_cpu)
-{
-	struct spm_driver_data *drv = NULL;
-	struct device_node *cpu_node, *saw_node;
-	int cpu;
-	bool found = 0;
+	spm = dev_get_drvdata(&pdev->dev);
+	if (!spm)
+		return -EINVAL;
 
-	for_each_possible_cpu(cpu) {
-		cpu_node = of_cpu_device_node_get(cpu);
-		if (!cpu_node)
-			continue;
-		saw_node = of_parse_phandle(cpu_node, "qcom,saw", 0);
-		found = (saw_node == pdev->dev.of_node);
-		of_node_put(saw_node);
-		of_node_put(cpu_node);
-		if (found)
-			break;
-	}
+	spm->cpuidle_driver = qcom_spm_idle_driver;
 
-	if (found) {
-		drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
-		if (drv)
-			*spm_cpu = cpu;
-	}
+	ret = dt_init_idle_driver(&spm->cpuidle_driver,
+				  qcom_idle_state_match, 1);
+	if (ret <= 0)
+		return ret ? : -ENODEV;
 
-	return drv;
-}
+	ret = qcom_scm_set_warm_boot_addr(cpu_resume_arm, cpumask_of(cpu));
+	if (ret)
+		return ret;
 
-static const struct of_device_id spm_match_table[] = {
-	{ .compatible = "qcom,msm8974-saw2-v2.1-cpu",
-	  .data = &spm_reg_8974_8084_cpu },
-	{ .compatible = "qcom,apq8084-saw2-v2.1-cpu",
-	  .data = &spm_reg_8974_8084_cpu },
-	{ .compatible = "qcom,apq8064-saw2-v1.1-cpu",
-	  .data = &spm_reg_8064_cpu },
-	{ },
-};
+	return cpuidle_register(&spm->cpuidle_driver, NULL);
+}
 
-static int spm_dev_probe(struct platform_device *pdev)
+static int spm_cpuidle_drv_probe(struct platform_device *pdev)
 {
-	struct spm_driver_data *drv;
-	struct resource *res;
-	const struct of_device_id *match_id;
-	void __iomem *addr;
 	int cpu, ret;
 
 	if (!qcom_scm_is_available())
 		return -EPROBE_DEFER;
 
-	drv = spm_get_drv(pdev, &cpu);
-	if (!drv)
-		return -EINVAL;
-	platform_set_drvdata(pdev, drv);
+	for_each_possible_cpu(cpu) {
+		ret = spm_cpuidle_register(cpu);
+		if (ret != -ENODEV) {
+			dev_err(&pdev->dev,
+				"Cannot register for CPU%d: %d\n", cpu, ret);
+			break;
+		}
+	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	drv->reg_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(drv->reg_base))
-		return PTR_ERR(drv->reg_base);
+	return ret;
+}
 
-	match_id = of_match_node(spm_match_table, pdev->dev.of_node);
-	if (!match_id)
-		return -ENODEV;
+static struct platform_driver spm_cpuidle_driver = {
+	.probe = spm_cpuidle_drv_probe,
+	.driver = {
+		.name = "qcom-spm-cpuidle",
+	},
+};
 
-	drv->reg_data = match_id->data;
+static int __init qcom_spm_cpuidle_init(void)
+{
+	struct platform_device *pdev;
+	int ret;
 
-	ret = spm_cpuidle_init(&drv->cpuidle_driver, cpu);
+	ret = platform_driver_register(&spm_cpuidle_driver);
 	if (ret)
 		return ret;
 
-	/* Write the SPM sequences first.. */
-	addr = drv->reg_base + drv->reg_data->reg_offset[SPM_REG_SEQ_ENTRY];
-	__iowrite32_copy(addr, drv->reg_data->seq,
-			ARRAY_SIZE(drv->reg_data->seq) / 4);
-
-	/*
-	 * ..and then the control registers.
-	 * On some SoC if the control registers are written first and if the
-	 * CPU was held in reset, the reset signal could trigger the SPM state
-	 * machine, before the sequences are completely written.
-	 */
-	spm_register_write(drv, SPM_REG_CFG, drv->reg_data->spm_cfg);
-	spm_register_write(drv, SPM_REG_DLY, drv->reg_data->spm_dly);
-	spm_register_write(drv, SPM_REG_PMIC_DLY, drv->reg_data->pmic_dly);
-	spm_register_write(drv, SPM_REG_PMIC_DATA_0,
-				drv->reg_data->pmic_data[0]);
-	spm_register_write(drv, SPM_REG_PMIC_DATA_1,
-				drv->reg_data->pmic_data[1]);
-
-	/* Set up Standby as the default low power mode */
-	spm_set_low_power_mode(drv, PM_SLEEP_MODE_STBY);
-
-	return cpuidle_register(&drv->cpuidle_driver, NULL);
-}
-
-static int spm_dev_remove(struct platform_device *pdev)
-{
-	struct spm_driver_data *drv = platform_get_drvdata(pdev);
+	pdev = platform_device_register_simple("qcom-spm-cpuidle",
+					       -1, NULL, 0);
+	if (IS_ERR(pdev)) {
+		platform_driver_unregister(&spm_cpuidle_driver);
+		return PTR_ERR(pdev);
+	}
 
-	cpuidle_unregister(&drv->cpuidle_driver);
 	return 0;
 }
-
-static struct platform_driver spm_driver = {
-	.probe = spm_dev_probe,
-	.remove = spm_dev_remove,
-	.driver = {
-		.name = "saw",
-		.of_match_table = spm_match_table,
-	},
-};
-
-builtin_platform_driver(spm_driver);
+device_initcall(qcom_spm_cpuidle_init);
diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index 79b568f82a1c..fe3c486ae32d 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -190,6 +190,15 @@  config QCOM_SOCINFO
 	 Say yes here to support the Qualcomm socinfo driver, providing
 	 information about the SoC to user space.
 
+config QCOM_SPM
+	tristate "Qualcomm Subsystem Power Manager (SPM)"
+	depends on ARCH_QCOM
+	select QCOM_SCM
+	help
+	  Enable the support for the Qualcomm Subsystem Power Manager, used
+	  to manage cores, L2 low power modes and to configure the internal
+	  Adaptive Voltage Scaler parameters, where supported.
+
 config QCOM_WCNSS_CTRL
 	tristate "Qualcomm WCNSS control driver"
 	depends on ARCH_QCOM || COMPILE_TEST
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index ad675a6593d0..24514c722832 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -20,6 +20,7 @@  obj-$(CONFIG_QCOM_SMEM_STATE) += smem_state.o
 obj-$(CONFIG_QCOM_SMP2P)	+= smp2p.o
 obj-$(CONFIG_QCOM_SMSM)	+= smsm.o
 obj-$(CONFIG_QCOM_SOCINFO)	+= socinfo.o
+obj-$(CONFIG_QCOM_SPM)		+= spm.o
 obj-$(CONFIG_QCOM_WCNSS_CTRL) += wcnss_ctrl.o
 obj-$(CONFIG_QCOM_APR) += apr.o
 obj-$(CONFIG_QCOM_LLCC) += llcc-qcom.o
diff --git a/drivers/soc/qcom/spm.c b/drivers/soc/qcom/spm.c
new file mode 100644
index 000000000000..0c8aa9240c41
--- /dev/null
+++ b/drivers/soc/qcom/spm.c
@@ -0,0 +1,198 @@ 
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2014,2015, Linaro Ltd.
+ *
+ * SAW power controller driver
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <soc/qcom/spm.h>
+
+#define SPM_CTL_INDEX		0x7f
+#define SPM_CTL_INDEX_SHIFT	4
+#define SPM_CTL_EN		BIT(0)
+
+enum spm_reg {
+	SPM_REG_CFG,
+	SPM_REG_SPM_CTL,
+	SPM_REG_DLY,
+	SPM_REG_PMIC_DLY,
+	SPM_REG_PMIC_DATA_0,
+	SPM_REG_PMIC_DATA_1,
+	SPM_REG_VCTL,
+	SPM_REG_SEQ_ENTRY,
+	SPM_REG_SPM_STS,
+	SPM_REG_PMIC_STS,
+	SPM_REG_NR,
+};
+
+static const u16 spm_reg_offset_v2_1[SPM_REG_NR] = {
+	[SPM_REG_CFG]		= 0x08,
+	[SPM_REG_SPM_CTL]	= 0x30,
+	[SPM_REG_DLY]		= 0x34,
+	[SPM_REG_SEQ_ENTRY]	= 0x80,
+};
+
+/* SPM register data for 8974, 8084 */
+static const struct spm_reg_data spm_reg_8974_8084_cpu  = {
+	.reg_offset = spm_reg_offset_v2_1,
+	.spm_cfg = 0x1,
+	.spm_dly = 0x3C102800,
+	.seq = { 0x03, 0x0B, 0x0F, 0x00, 0x20, 0x80, 0x10, 0xE8, 0x5B, 0x03,
+		0x3B, 0xE8, 0x5B, 0x82, 0x10, 0x0B, 0x30, 0x06, 0x26, 0x30,
+		0x0F },
+	.start_index[PM_SLEEP_MODE_STBY] = 0,
+	.start_index[PM_SLEEP_MODE_SPC] = 3,
+};
+
+static const u16 spm_reg_offset_v1_1[SPM_REG_NR] = {
+	[SPM_REG_CFG]		= 0x08,
+	[SPM_REG_SPM_CTL]	= 0x20,
+	[SPM_REG_PMIC_DLY]	= 0x24,
+	[SPM_REG_PMIC_DATA_0]	= 0x28,
+	[SPM_REG_PMIC_DATA_1]	= 0x2C,
+	[SPM_REG_SEQ_ENTRY]	= 0x80,
+};
+
+/* SPM register data for 8064 */
+static const struct spm_reg_data spm_reg_8064_cpu = {
+	.reg_offset = spm_reg_offset_v1_1,
+	.spm_cfg = 0x1F,
+	.pmic_dly = 0x02020004,
+	.pmic_data[0] = 0x0084009C,
+	.pmic_data[1] = 0x00A4001C,
+	.seq = { 0x03, 0x0F, 0x00, 0x24, 0x54, 0x10, 0x09, 0x03, 0x01,
+		0x10, 0x54, 0x30, 0x0C, 0x24, 0x30, 0x0F },
+	.start_index[PM_SLEEP_MODE_STBY] = 0,
+	.start_index[PM_SLEEP_MODE_SPC] = 2,
+};
+
+static inline void spm_register_write(struct spm_driver_data *drv,
+					enum spm_reg reg, u32 val)
+{
+	if (drv->reg_data->reg_offset[reg])
+		writel_relaxed(val, drv->reg_base +
+				drv->reg_data->reg_offset[reg]);
+}
+
+/* Ensure a guaranteed write, before return */
+static inline void spm_register_write_sync(struct spm_driver_data *drv,
+					enum spm_reg reg, u32 val)
+{
+	u32 ret;
+
+	if (!drv->reg_data->reg_offset[reg])
+		return;
+
+	do {
+		writel_relaxed(val, drv->reg_base +
+				drv->reg_data->reg_offset[reg]);
+		ret = readl_relaxed(drv->reg_base +
+				drv->reg_data->reg_offset[reg]);
+		if (ret == val)
+			break;
+		cpu_relax();
+	} while (1);
+}
+
+static inline u32 spm_register_read(struct spm_driver_data *drv,
+					enum spm_reg reg)
+{
+	return readl_relaxed(drv->reg_base + drv->reg_data->reg_offset[reg]);
+}
+
+void spm_set_low_power_mode(struct spm_driver_data *drv,
+					enum pm_sleep_mode mode)
+{
+	u32 start_index;
+	u32 ctl_val;
+
+	start_index = drv->reg_data->start_index[mode];
+
+	ctl_val = spm_register_read(drv, SPM_REG_SPM_CTL);
+	ctl_val &= ~(SPM_CTL_INDEX << SPM_CTL_INDEX_SHIFT);
+	ctl_val |= start_index << SPM_CTL_INDEX_SHIFT;
+	ctl_val |= SPM_CTL_EN;
+	spm_register_write_sync(drv, SPM_REG_SPM_CTL, ctl_val);
+}
+
+static const struct of_device_id spm_match_table[] = {
+	{ .compatible = "qcom,msm8974-saw2-v2.1-cpu",
+	  .data = &spm_reg_8974_8084_cpu },
+	{ .compatible = "qcom,apq8084-saw2-v2.1-cpu",
+	  .data = &spm_reg_8974_8084_cpu },
+	{ .compatible = "qcom,apq8064-saw2-v1.1-cpu",
+	  .data = &spm_reg_8064_cpu },
+	{ },
+};
+
+static int spm_dev_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match_id;
+	struct spm_driver_data *drv;
+	struct resource *res;
+	void __iomem *addr;
+
+	drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
+	if (!drv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	drv->reg_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(drv->reg_base))
+		return PTR_ERR(drv->reg_base);
+
+	match_id = of_match_node(spm_match_table, pdev->dev.of_node);
+	if (!match_id)
+		return -ENODEV;
+
+	drv->reg_data = match_id->data;
+	platform_set_drvdata(pdev, drv);
+
+	/* Write the SPM sequences first.. */
+	addr = drv->reg_base + drv->reg_data->reg_offset[SPM_REG_SEQ_ENTRY];
+	__iowrite32_copy(addr, drv->reg_data->seq,
+			ARRAY_SIZE(drv->reg_data->seq) / 4);
+
+	/*
+	 * ..and then the control registers.
+	 * On some SoC if the control registers are written first and if the
+	 * CPU was held in reset, the reset signal could trigger the SPM state
+	 * machine, before the sequences are completely written.
+	 */
+	spm_register_write(drv, SPM_REG_CFG, drv->reg_data->spm_cfg);
+	spm_register_write(drv, SPM_REG_DLY, drv->reg_data->spm_dly);
+	spm_register_write(drv, SPM_REG_PMIC_DLY, drv->reg_data->pmic_dly);
+	spm_register_write(drv, SPM_REG_PMIC_DATA_0,
+				drv->reg_data->pmic_data[0]);
+	spm_register_write(drv, SPM_REG_PMIC_DATA_1,
+				drv->reg_data->pmic_data[1]);
+
+	/* Set up Standby as the default low power mode */
+	spm_set_low_power_mode(drv, PM_SLEEP_MODE_STBY);
+
+	return 0;
+}
+
+static struct platform_driver spm_driver = {
+	.probe = spm_dev_probe,
+	.driver = {
+		.name = "qcom_spm",
+		.of_match_table = spm_match_table,
+	},
+};
+
+static int __init qcom_spm_init(void)
+{
+	return platform_driver_register(&spm_driver);
+}
+arch_initcall(qcom_spm_init);
diff --git a/include/soc/qcom/spm.h b/include/soc/qcom/spm.h
new file mode 100644
index 000000000000..604eca2c4d4a
--- /dev/null
+++ b/include/soc/qcom/spm.h
@@ -0,0 +1,45 @@ 
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2014,2015, Linaro Ltd.
+ * Copyright (C) 2020, AngeloGioacchino Del Regno <kholk11@gmail.com>
+ */
+
+#ifndef __SPM_H__
+#define __SPM_H__
+
+#include <linux/cpuidle.h>
+
+#define MAX_PMIC_DATA		2
+#define MAX_SEQ_DATA		64
+
+enum pm_sleep_mode {
+	PM_SLEEP_MODE_STBY,
+	PM_SLEEP_MODE_RET,
+	PM_SLEEP_MODE_SPC,
+	PM_SLEEP_MODE_PC,
+	PM_SLEEP_MODE_NR,
+};
+
+struct spm_reg_data {
+	const u16 *reg_offset;
+	u32 spm_cfg;
+	u32 spm_dly;
+	u32 pmic_dly;
+	u32 pmic_data[MAX_PMIC_DATA];
+	u32 avs_ctl;
+	u32 avs_limit;
+	u8 seq[MAX_SEQ_DATA];
+	u8 start_index[PM_SLEEP_MODE_NR];
+};
+
+struct spm_driver_data {
+	struct cpuidle_driver cpuidle_driver;
+	void __iomem *reg_base;
+	const struct spm_reg_data *reg_data;
+};
+
+void spm_set_low_power_mode(struct spm_driver_data *drv,
+					enum pm_sleep_mode mode);
+
+#endif /* __SPM_H__ */