[PATCH RFC v3 00/12] arm64: suspend/resume implementation

[PATCH RFC v3 00/12] arm64: suspend/resume implementation

[Lorenzo Pieralisi]

This series is v3 of a previous posting:

http://lists.infradead.org/pipermail/linux-arm-kernel/2013-October/204471.html

CPU idle driver and PSCI suspend implementation used for testing are not
published as part of this series since they depend on standardization
of PSCI bindings for C-states; code can be made available on a separate branch
if required.

v3 changes:

- Rebased to 3.12
- Removed unused macros, merged them in generic cpu_do_{suspend}/{resume}
- Refactored hw breakpoint reset code
- Split Kconfig and make infrastructure
- Moved debug monitors save/restore to cpu_do_{suspend}/{resume} and
  cpu_suspend interface

v2 changes:

- Rebased to 3.12-rc4
- Changed cpu_suspend prototype
- Added macros to save/restore CPU registers
- Improved register/load/store scheduling in context save/restore
- Changed Kconfig option
- Refactored the stack allocation and context struct layout
- Fixed a bug in cpu_suspend return path
- Added tick broadcast support
- Dropped RFC tag

This patch series provides an implementation of suspend/resume and
related CPU PM notifiers for the arm64 architecture. The implementation
has been tweaked and code made a bit more generic wrt ARM 32 bits
version. The cpu_suspend call prototype accepts a single parameter that
is translated by the CPU operations suspend protocol backend into a
series of protocol (eg PSCI) specific parameters and function calls.
This allows for generic suspend-to-RAM and CPU idle drivers, where the
platform complexity is hidden behind the CPU ops suspend protocol
implementation.

The context restore split between processor specific functions and
generic code has been kept similar to arm32 as well, since this makes
sense from a functionality perspective, even though there is just a
an architecture specific save/restore implementation for arm64, for v8.
Code that deals with turning on the MMU and returning from suspend is kept
in the same file as the the suspend method itself, so that assembly code is
self contained in a single file and separated from the interface.

The CPU register context is allocated on the stack; this behaviour is
the same as in the arm 32-bit port, even though context could have been
allocated statically as a NR_CPUS array of struct and the context
addresses stored once of all in the context pointers array after virtual
to physical conversion. Current solution uses the stack, since there is
no need to allocate additional context space for context that can be
easily saved on the suspend threads stacks, with no additional memory
requirement.

Cache flushing is kept to a minimum; the cpu_suspend method cleans only the
few bytes of context to DRAM that need to be retrieved with the MMU off,
the remainder of cache cleaning is delegated to suspend finishers.

The series also provides patches to simplify the MPIDR_EL1 management
and MPIDR_EL1 hashing that are integrated to the series but are also useful
as stand alone code.

Code has been tested on AEM v8 models and a simple CPU idle driver that
enables a C-state where CPUs are shutdown when wfi is hit.

KVM CPU PM notifier has been tested using kvm-tools, and by booting a KVM guest
on a host with deep idle states enabled.

HW breakpoint CPU PM notifier has been tested using a simple perf test that
sets up per-cpu watchpoints and checks the proper behaviour when deep idle
states are enabled.

Lorenzo Pieralisi (12):
  arm64: kernel: add MPIDR_EL1 accessors macros
  arm64: kernel: build MPIDR_EL1 hash function data structure
  arm64: kernel: suspend/resume registers save/restore
  arm64: kernel: cpu_{suspend/resume} implementation
  arm64: kernel: implement fpsimd CPU PM notifier
  arm: kvm: implement CPU PM notifier
  arm64: kernel: refactor code to install/uninstall breakpoints
  arm64: kernel: implement HW breakpoints CPU PM notifier
  arm64: enable generic clockevent broadcast
  arm64: kernel: add CPU idle call
  arm64: kernel: add PM build infrastructure
  arm64: add CPU power management menu/entries

 arch/arm/kvm/arm.c                |  30 ++++++
 arch/arm64/Kconfig                |  21 ++++
 arch/arm64/include/asm/cpu_ops.h  |   6 ++
 arch/arm64/include/asm/cputype.h  |  10 ++
 arch/arm64/include/asm/hardirq.h  |   2 +-
 arch/arm64/include/asm/proc-fns.h |   3 +
 arch/arm64/include/asm/smp_plat.h |  13 +++
 arch/arm64/include/asm/suspend.h  |  27 +++++
 arch/arm64/kernel/Makefile        |   1 +
 arch/arm64/kernel/asm-offsets.c   |  11 ++
 arch/arm64/kernel/fpsimd.c        |  36 +++++++
 arch/arm64/kernel/hw_breakpoint.c | 221 ++++++++++++++++++++++++++------------
 arch/arm64/kernel/process.c       |   7 +-
 arch/arm64/kernel/setup.c         |  70 ++++++++++++
 arch/arm64/kernel/sleep.S         | 184 +++++++++++++++++++++++++++++++
 arch/arm64/kernel/smp.c           |  17 +++
 arch/arm64/kernel/suspend.c       | 109 +++++++++++++++++++
 arch/arm64/mm/proc.S              |  69 ++++++++++++
 18 files changed, 767 insertions(+), 70 deletions(-)
 create mode 100644 arch/arm64/include/asm/suspend.h
 create mode 100644 arch/arm64/kernel/sleep.S
 create mode 100644 arch/arm64/kernel/suspend.c

-- 
1.8.4

[PATCH RFC v3 00/12] arm64: suspend/resume implementation

[Lorenzo Pieralisi]

This series is v3 of a previous posting:

http://lists.infradead.org/pipermail/linux-arm-kernel/2013-October/204471.html

CPU idle driver and PSCI suspend implementation used for testing are not
published as part of this series since they depend on standardization
of PSCI bindings for C-states; code can be made available on a separate branch
if required.

v3 changes:

- Rebased to 3.12
- Removed unused macros, merged them in generic cpu_do_{suspend}/{resume}
- Refactored hw breakpoint reset code
- Split Kconfig and make infrastructure
- Moved debug monitors save/restore to cpu_do_{suspend}/{resume} and
  cpu_suspend interface

v2 changes:

- Rebased to 3.12-rc4
- Changed cpu_suspend prototype
- Added macros to save/restore CPU registers
- Improved register/load/store scheduling in context save/restore
- Changed Kconfig option
- Refactored the stack allocation and context struct layout
- Fixed a bug in cpu_suspend return path
- Added tick broadcast support
- Dropped RFC tag

This patch series provides an implementation of suspend/resume and
related CPU PM notifiers for the arm64 architecture. The implementation
has been tweaked and code made a bit more generic wrt ARM 32 bits
version. The cpu_suspend call prototype accepts a single parameter that
is translated by the CPU operations suspend protocol backend into a
series of protocol (eg PSCI) specific parameters and function calls.
This allows for generic suspend-to-RAM and CPU idle drivers, where the
platform complexity is hidden behind the CPU ops suspend protocol
implementation.

The context restore split between processor specific functions and
generic code has been kept similar to arm32 as well, since this makes
sense from a functionality perspective, even though there is just a
an architecture specific save/restore implementation for arm64, for v8.
Code that deals with turning on the MMU and returning from suspend is kept
in the same file as the the suspend method itself, so that assembly code is
self contained in a single file and separated from the interface.

The CPU register context is allocated on the stack; this behaviour is
the same as in the arm 32-bit port, even though context could have been
allocated statically as a NR_CPUS array of struct and the context
addresses stored once of all in the context pointers array after virtual
to physical conversion. Current solution uses the stack, since there is
no need to allocate additional context space for context that can be
easily saved on the suspend threads stacks, with no additional memory
requirement.

Cache flushing is kept to a minimum; the cpu_suspend method cleans only the
few bytes of context to DRAM that need to be retrieved with the MMU off,
the remainder of cache cleaning is delegated to suspend finishers.

The series also provides patches to simplify the MPIDR_EL1 management
and MPIDR_EL1 hashing that are integrated to the series but are also useful
as stand alone code.

Code has been tested on AEM v8 models and a simple CPU idle driver that
enables a C-state where CPUs are shutdown when wfi is hit.

KVM CPU PM notifier has been tested using kvm-tools, and by booting a KVM guest
on a host with deep idle states enabled.

HW breakpoint CPU PM notifier has been tested using a simple perf test that
sets up per-cpu watchpoints and checks the proper behaviour when deep idle
states are enabled.

Lorenzo Pieralisi (12):
  arm64: kernel: add MPIDR_EL1 accessors macros
  arm64: kernel: build MPIDR_EL1 hash function data structure
  arm64: kernel: suspend/resume registers save/restore
  arm64: kernel: cpu_{suspend/resume} implementation
  arm64: kernel: implement fpsimd CPU PM notifier
  arm: kvm: implement CPU PM notifier
  arm64: kernel: refactor code to install/uninstall breakpoints
  arm64: kernel: implement HW breakpoints CPU PM notifier
  arm64: enable generic clockevent broadcast
  arm64: kernel: add CPU idle call
  arm64: kernel: add PM build infrastructure
  arm64: add CPU power management menu/entries

 arch/arm/kvm/arm.c                |  30 ++++++
 arch/arm64/Kconfig                |  21 ++++
 arch/arm64/include/asm/cpu_ops.h  |   6 ++
 arch/arm64/include/asm/cputype.h  |  10 ++
 arch/arm64/include/asm/hardirq.h  |   2 +-
 arch/arm64/include/asm/proc-fns.h |   3 +
 arch/arm64/include/asm/smp_plat.h |  13 +++
 arch/arm64/include/asm/suspend.h  |  27 +++++
 arch/arm64/kernel/Makefile        |   1 +
 arch/arm64/kernel/asm-offsets.c   |  11 ++
 arch/arm64/kernel/fpsimd.c        |  36 +++++++
 arch/arm64/kernel/hw_breakpoint.c | 221 ++++++++++++++++++++++++++------------
 arch/arm64/kernel/process.c       |   7 +-
 arch/arm64/kernel/setup.c         |  70 ++++++++++++
 arch/arm64/kernel/sleep.S         | 184 +++++++++++++++++++++++++++++++
 arch/arm64/kernel/smp.c           |  17 +++
 arch/arm64/kernel/suspend.c       | 109 +++++++++++++++++++
 arch/arm64/mm/proc.S              |  69 ++++++++++++
 18 files changed, 767 insertions(+), 70 deletions(-)
 create mode 100644 arch/arm64/include/asm/suspend.h
 create mode 100644 arch/arm64/kernel/sleep.S
 create mode 100644 arch/arm64/kernel/suspend.c

-- 
1.8.4

[PATCH RFC v3 01/12] arm64: kernel: add MPIDR_EL1 accessors macros

[Lorenzo Pieralisi]

In order to simplify access to different affinity levels within the
MPIDR_EL1 register values, this patch implements some preprocessor
macros that allow to retrieve the MPIDR_EL1 affinity level value according
to the level passed as input parameter.

Reviewed-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/cputype.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/arch/arm64/include/asm/cputype.h b/arch/arm64/include/asm/cputype.h
index 5fe138e..e371936 100644
--- a/arch/arm64/include/asm/cputype.h
+++ b/arch/arm64/include/asm/cputype.h
@@ -30,6 +30,16 @@
 
 #define MPIDR_HWID_BITMASK	0xff00ffffff
 
+#define MPIDR_LEVEL_BITS_SHIFT	3
+#define MPIDR_LEVEL_BITS	(1 << MPIDR_LEVEL_BITS_SHIFT)
+#define MPIDR_LEVEL_MASK	((1 << MPIDR_LEVEL_BITS) - 1)
+
+#define MPIDR_LEVEL_SHIFT(level) \
+	(((1 << level) >> 1) << MPIDR_LEVEL_BITS_SHIFT)
+
+#define MPIDR_AFFINITY_LEVEL(mpidr, level) \
+	((mpidr >> MPIDR_LEVEL_SHIFT(level)) & MPIDR_LEVEL_MASK)
+
 #define read_cpuid(reg) ({						\
 	u64 __val;							\
 	asm("mrs	%0, " reg : "=r" (__val));			\
-- 
1.8.4

[PATCH RFC v3 01/12] arm64: kernel: add MPIDR_EL1 accessors macros

[Lorenzo Pieralisi]

In order to simplify access to different affinity levels within the
MPIDR_EL1 register values, this patch implements some preprocessor
macros that allow to retrieve the MPIDR_EL1 affinity level value according
to the level passed as input parameter.

Reviewed-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/cputype.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/arch/arm64/include/asm/cputype.h b/arch/arm64/include/asm/cputype.h
index 5fe138e..e371936 100644
--- a/arch/arm64/include/asm/cputype.h
+++ b/arch/arm64/include/asm/cputype.h
@@ -30,6 +30,16 @@
 
 #define MPIDR_HWID_BITMASK	0xff00ffffff
 
+#define MPIDR_LEVEL_BITS_SHIFT	3
+#define MPIDR_LEVEL_BITS	(1 << MPIDR_LEVEL_BITS_SHIFT)
+#define MPIDR_LEVEL_MASK	((1 << MPIDR_LEVEL_BITS) - 1)
+
+#define MPIDR_LEVEL_SHIFT(level) \
+	(((1 << level) >> 1) << MPIDR_LEVEL_BITS_SHIFT)
+
+#define MPIDR_AFFINITY_LEVEL(mpidr, level) \
+	((mpidr >> MPIDR_LEVEL_SHIFT(level)) & MPIDR_LEVEL_MASK)
+
 #define read_cpuid(reg) ({						\
 	u64 __val;							\
 	asm("mrs	%0, " reg : "=r" (__val));			\
-- 
1.8.4

[PATCH RFC v3 02/12] arm64: kernel: build MPIDR_EL1 hash function data structure

[Lorenzo Pieralisi]

On ARM64 SMP systems, cores are identified by their MPIDR_EL1 register.
The MPIDR_EL1 guidelines in the ARM ARM do not provide strict enforcement of
MPIDR_EL1 layout, only recommendations that, if followed, split the MPIDR_EL1
on ARM 64 bit platforms in four affinity levels. In multi-cluster
systems like big.LITTLE, if the affinity guidelines are followed, the
MPIDR_EL1 can not be considered a linear index. This means that the
association between logical CPU in the kernel and the HW CPU identifier
becomes somewhat more complicated requiring methods like hashing to
associate a given MPIDR_EL1 to a CPU logical index, in order for the look-up
to be carried out in an efficient and scalable way.

This patch provides a function in the kernel that starting from the
cpu_logical_map, implement collision-free hashing of MPIDR_EL1 values by
checking all significative bits of MPIDR_EL1 affinity level bitfields.
The hashing can then be carried out through bits shifting and ORing; the
resulting hash algorithm is a collision-free though not minimal hash that can
be executed with few assembly instructions. The mpidr_el1 is filtered through a
mpidr mask that is built by checking all bits that toggle in the set of
MPIDR_EL1s corresponding to possible CPUs. Bits that do not toggle do not
carry information so they do not contribute to the resulting hash.

Pseudo code:

/* check all bits that toggle, so they are required */
for (i = 1, mpidr_el1_mask = 0; i < num_possible_cpus(); i++)
	mpidr_el1_mask |= (cpu_logical_map(i) ^ cpu_logical_map(0));

/*
 * Build shifts to be applied to aff0, aff1, aff2, aff3 values to hash the
 * mpidr_el1
 * fls() returns the last bit set in a word, 0 if none
 * ffs() returns the first bit set in a word, 0 if none
 */
fs0 = mpidr_el1_mask[7:0] ? ffs(mpidr_el1_mask[7:0]) - 1 : 0;
fs1 = mpidr_el1_mask[15:8] ? ffs(mpidr_el1_mask[15:8]) - 1 : 0;
fs2 = mpidr_el1_mask[23:16] ? ffs(mpidr_el1_mask[23:16]) - 1 : 0;
fs3 = mpidr_el1_mask[39:32] ? ffs(mpidr_el1_mask[39:32]) - 1 : 0;
ls0 = fls(mpidr_el1_mask[7:0]);
ls1 = fls(mpidr_el1_mask[15:8]);
ls2 = fls(mpidr_el1_mask[23:16]);
ls3 = fls(mpidr_el1_mask[39:32]);
bits0 = ls0 - fs0;
bits1 = ls1 - fs1;
bits2 = ls2 - fs2;
bits3 = ls3 - fs3;
aff0_shift = fs0;
aff1_shift = 8 + fs1 - bits0;
aff2_shift = 16 + fs2 - (bits0 + bits1);
aff3_shift = 32 + fs3 - (bits0 + bits1 + bits2);
u32 hash(u64 mpidr_el1) {
	u32 l[4];
	u64 mpidr_el1_masked = mpidr_el1 & mpidr_el1_mask;
	l[0] = mpidr_el1_masked & 0xff;
	l[1] = mpidr_el1_masked & 0xff00;
	l[2] = mpidr_el1_masked & 0xff0000;
	l[3] = mpidr_el1_masked & 0xff00000000;
	return (l[0] >> aff0_shift | l[1] >> aff1_shift | l[2] >> aff2_shift |
		l[3] >> aff3_shift);
}

The hashing algorithm relies on the inherent properties set in the ARM ARM
recommendations for the MPIDR_EL1. Exotic configurations, where for instance
the MPIDR_EL1 values at a given affinity level have large holes, can end up
requiring big hash tables since the compression of values that can be achieved
through shifting is somewhat crippled when holes are present. Kernel warns if
the number of buckets of the resulting hash table exceeds the number of
possible CPUs by a factor of 4, which is a symptom of a very sparse HW
MPIDR_EL1 configuration.

The hash algorithm is quite simple and can easily be implemented in assembly
code, to be used in code paths where the kernel virtual address space is
not set-up (ie cpu_resume) and instruction and data fetches are strongly
ordered so code must be compact and must carry out few data accesses.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/smp_plat.h | 13 ++++++++
 arch/arm64/kernel/setup.c         | 70 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 83 insertions(+)

diff --git a/arch/arm64/include/asm/smp_plat.h b/arch/arm64/include/asm/smp_plat.h
index ed43a0d..59e2823 100644
--- a/arch/arm64/include/asm/smp_plat.h
+++ b/arch/arm64/include/asm/smp_plat.h
@@ -21,6 +21,19 @@
 
 #include <asm/types.h>
 
+struct mpidr_hash {
+	u64	mask;
+	u32	shift_aff[4];
+	u32	bits;
+};
+
+extern struct mpidr_hash mpidr_hash;
+
+static inline u32 mpidr_hash_size(void)
+{
+	return 1 << mpidr_hash.bits;
+}
+
 /*
  * Logical CPU mapping.
  */
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 0bc5e4c..e91d33e 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -113,6 +113,75 @@ bool arch_match_cpu_phys_id(int cpu, u64 phys_id)
 	return phys_id == cpu_logical_map(cpu);
 }
 
+struct mpidr_hash mpidr_hash;
+#ifdef CONFIG_SMP
+/**
+ * smp_build_mpidr_hash - Pre-compute shifts required at each affinity
+ *			  level in order to build a linear index from an
+ *			  MPIDR value. Resulting algorithm is a collision
+ *			  free hash carried out through shifting and ORing
+ */
+static void __init smp_build_mpidr_hash(void)
+{
+	u32 i, affinity, fs[4], bits[4], ls;
+	u64 mask = 0;
+	/*
+	 * Pre-scan the list of MPIDRS and filter out bits that do
+	 * not contribute to affinity levels, ie they never toggle.
+	 */
+	for_each_possible_cpu(i)
+		mask |= (cpu_logical_map(i) ^ cpu_logical_map(0));
+	pr_debug("mask of set bits %#llx\n", mask);
+	/*
+	 * Find and stash the last and first bit set at all affinity levels to
+	 * check how many bits are required to represent them.
+	 */
+	for (i = 0; i < 4; i++) {
+		affinity = MPIDR_AFFINITY_LEVEL(mask, i);
+		/*
+		 * Find the MSB bit and LSB bits position
+		 * to determine how many bits are required
+		 * to express the affinity level.
+		 */
+		ls = fls(affinity);
+		fs[i] = affinity ? ffs(affinity) - 1 : 0;
+		bits[i] = ls - fs[i];
+	}
+	/*
+	 * An index can be created from the MPIDR_EL1 by isolating the
+	 * significant bits at each affinity level and by shifting
+	 * them in order to compress the 32 bits values space to a
+	 * compressed set of values. This is equivalent to hashing
+	 * the MPIDR_EL1 through shifting and ORing. It is a collision free
+	 * hash though not minimal since some levels might contain a number
+	 * of CPUs that is not an exact power of 2 and their bit
+	 * representation might contain holes, eg MPIDR_EL1[7:0] = {0x2, 0x80}.
+	 */
+	mpidr_hash.shift_aff[0] = MPIDR_LEVEL_SHIFT(0) + fs[0];
+	mpidr_hash.shift_aff[1] = MPIDR_LEVEL_SHIFT(1) + fs[1] - bits[0];
+	mpidr_hash.shift_aff[2] = MPIDR_LEVEL_SHIFT(2) + fs[2] -
+						(bits[1] + bits[0]);
+	mpidr_hash.shift_aff[3] = MPIDR_LEVEL_SHIFT(3) +
+				  fs[3] - (bits[2] + bits[1] + bits[0]);
+	mpidr_hash.mask = mask;
+	mpidr_hash.bits = bits[3] + bits[2] + bits[1] + bits[0];
+	pr_debug("MPIDR hash: aff0[%u] aff1[%u] aff2[%u] aff3[%u] mask[%#llx] bits[%u]\n",
+		mpidr_hash.shift_aff[0],
+		mpidr_hash.shift_aff[1],
+		mpidr_hash.shift_aff[2],
+		mpidr_hash.shift_aff[3],
+		mpidr_hash.mask,
+		mpidr_hash.bits);
+	/*
+	 * 4x is an arbitrary value used to warn on a hash table much bigger
+	 * than expected on most systems.
+	 */
+	if (mpidr_hash_size() > 4 * num_possible_cpus())
+		pr_warn("Large number of MPIDR hash buckets detected\n");
+	__flush_dcache_area(&mpidr_hash, sizeof(struct mpidr_hash));
+}
+#endif
+
 static void __init setup_processor(void)
 {
 	struct cpu_info *cpu_info;
@@ -231,6 +300,7 @@ void __init setup_arch(char **cmdline_p)
 	cpu_read_bootcpu_ops();
 #ifdef CONFIG_SMP
 	smp_init_cpus();
+	smp_build_mpidr_hash();
 #endif
 
 #ifdef CONFIG_VT
-- 
1.8.4

[PATCH RFC v3 02/12] arm64: kernel: build MPIDR_EL1 hash function data structure

[Lorenzo Pieralisi]

On ARM64 SMP systems, cores are identified by their MPIDR_EL1 register.
The MPIDR_EL1 guidelines in the ARM ARM do not provide strict enforcement of
MPIDR_EL1 layout, only recommendations that, if followed, split the MPIDR_EL1
on ARM 64 bit platforms in four affinity levels. In multi-cluster
systems like big.LITTLE, if the affinity guidelines are followed, the
MPIDR_EL1 can not be considered a linear index. This means that the
association between logical CPU in the kernel and the HW CPU identifier
becomes somewhat more complicated requiring methods like hashing to
associate a given MPIDR_EL1 to a CPU logical index, in order for the look-up
to be carried out in an efficient and scalable way.

This patch provides a function in the kernel that starting from the
cpu_logical_map, implement collision-free hashing of MPIDR_EL1 values by
checking all significative bits of MPIDR_EL1 affinity level bitfields.
The hashing can then be carried out through bits shifting and ORing; the
resulting hash algorithm is a collision-free though not minimal hash that can
be executed with few assembly instructions. The mpidr_el1 is filtered through a
mpidr mask that is built by checking all bits that toggle in the set of
MPIDR_EL1s corresponding to possible CPUs. Bits that do not toggle do not
carry information so they do not contribute to the resulting hash.

Pseudo code:

/* check all bits that toggle, so they are required */
for (i = 1, mpidr_el1_mask = 0; i < num_possible_cpus(); i++)
	mpidr_el1_mask |= (cpu_logical_map(i) ^ cpu_logical_map(0));

/*
 * Build shifts to be applied to aff0, aff1, aff2, aff3 values to hash the
 * mpidr_el1
 * fls() returns the last bit set in a word, 0 if none
 * ffs() returns the first bit set in a word, 0 if none
 */
fs0 = mpidr_el1_mask[7:0] ? ffs(mpidr_el1_mask[7:0]) - 1 : 0;
fs1 = mpidr_el1_mask[15:8] ? ffs(mpidr_el1_mask[15:8]) - 1 : 0;
fs2 = mpidr_el1_mask[23:16] ? ffs(mpidr_el1_mask[23:16]) - 1 : 0;
fs3 = mpidr_el1_mask[39:32] ? ffs(mpidr_el1_mask[39:32]) - 1 : 0;
ls0 = fls(mpidr_el1_mask[7:0]);
ls1 = fls(mpidr_el1_mask[15:8]);
ls2 = fls(mpidr_el1_mask[23:16]);
ls3 = fls(mpidr_el1_mask[39:32]);
bits0 = ls0 - fs0;
bits1 = ls1 - fs1;
bits2 = ls2 - fs2;
bits3 = ls3 - fs3;
aff0_shift = fs0;
aff1_shift = 8 + fs1 - bits0;
aff2_shift = 16 + fs2 - (bits0 + bits1);
aff3_shift = 32 + fs3 - (bits0 + bits1 + bits2);
u32 hash(u64 mpidr_el1) {
	u32 l[4];
	u64 mpidr_el1_masked = mpidr_el1 & mpidr_el1_mask;
	l[0] = mpidr_el1_masked & 0xff;
	l[1] = mpidr_el1_masked & 0xff00;
	l[2] = mpidr_el1_masked & 0xff0000;
	l[3] = mpidr_el1_masked & 0xff00000000;
	return (l[0] >> aff0_shift | l[1] >> aff1_shift | l[2] >> aff2_shift |
		l[3] >> aff3_shift);
}

The hashing algorithm relies on the inherent properties set in the ARM ARM
recommendations for the MPIDR_EL1. Exotic configurations, where for instance
the MPIDR_EL1 values at a given affinity level have large holes, can end up
requiring big hash tables since the compression of values that can be achieved
through shifting is somewhat crippled when holes are present. Kernel warns if
the number of buckets of the resulting hash table exceeds the number of
possible CPUs by a factor of 4, which is a symptom of a very sparse HW
MPIDR_EL1 configuration.

The hash algorithm is quite simple and can easily be implemented in assembly
code, to be used in code paths where the kernel virtual address space is
not set-up (ie cpu_resume) and instruction and data fetches are strongly
ordered so code must be compact and must carry out few data accesses.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/smp_plat.h | 13 ++++++++
 arch/arm64/kernel/setup.c         | 70 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 83 insertions(+)

diff --git a/arch/arm64/include/asm/smp_plat.h b/arch/arm64/include/asm/smp_plat.h
index ed43a0d..59e2823 100644
--- a/arch/arm64/include/asm/smp_plat.h
+++ b/arch/arm64/include/asm/smp_plat.h
@@ -21,6 +21,19 @@
 
 #include <asm/types.h>
 
+struct mpidr_hash {
+	u64	mask;
+	u32	shift_aff[4];
+	u32	bits;
+};
+
+extern struct mpidr_hash mpidr_hash;
+
+static inline u32 mpidr_hash_size(void)
+{
+	return 1 << mpidr_hash.bits;
+}
+
 /*
  * Logical CPU mapping.
  */
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 0bc5e4c..e91d33e 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -113,6 +113,75 @@ bool arch_match_cpu_phys_id(int cpu, u64 phys_id)
 	return phys_id == cpu_logical_map(cpu);
 }
 
+struct mpidr_hash mpidr_hash;
+#ifdef CONFIG_SMP
+/**
+ * smp_build_mpidr_hash - Pre-compute shifts required at each affinity
+ *			  level in order to build a linear index from an
+ *			  MPIDR value. Resulting algorithm is a collision
+ *			  free hash carried out through shifting and ORing
+ */
+static void __init smp_build_mpidr_hash(void)
+{
+	u32 i, affinity, fs[4], bits[4], ls;
+	u64 mask = 0;
+	/*
+	 * Pre-scan the list of MPIDRS and filter out bits that do
+	 * not contribute to affinity levels, ie they never toggle.
+	 */
+	for_each_possible_cpu(i)
+		mask |= (cpu_logical_map(i) ^ cpu_logical_map(0));
+	pr_debug("mask of set bits %#llx\n", mask);
+	/*
+	 * Find and stash the last and first bit set at all affinity levels to
+	 * check how many bits are required to represent them.
+	 */
+	for (i = 0; i < 4; i++) {
+		affinity = MPIDR_AFFINITY_LEVEL(mask, i);
+		/*
+		 * Find the MSB bit and LSB bits position
+		 * to determine how many bits are required
+		 * to express the affinity level.
+		 */
+		ls = fls(affinity);
+		fs[i] = affinity ? ffs(affinity) - 1 : 0;
+		bits[i] = ls - fs[i];
+	}
+	/*
+	 * An index can be created from the MPIDR_EL1 by isolating the
+	 * significant bits@each affinity level and by shifting
+	 * them in order to compress the 32 bits values space to a
+	 * compressed set of values. This is equivalent to hashing
+	 * the MPIDR_EL1 through shifting and ORing. It is a collision free
+	 * hash though not minimal since some levels might contain a number
+	 * of CPUs that is not an exact power of 2 and their bit
+	 * representation might contain holes, eg MPIDR_EL1[7:0] = {0x2, 0x80}.
+	 */
+	mpidr_hash.shift_aff[0] = MPIDR_LEVEL_SHIFT(0) + fs[0];
+	mpidr_hash.shift_aff[1] = MPIDR_LEVEL_SHIFT(1) + fs[1] - bits[0];
+	mpidr_hash.shift_aff[2] = MPIDR_LEVEL_SHIFT(2) + fs[2] -
+						(bits[1] + bits[0]);
+	mpidr_hash.shift_aff[3] = MPIDR_LEVEL_SHIFT(3) +
+				  fs[3] - (bits[2] + bits[1] + bits[0]);
+	mpidr_hash.mask = mask;
+	mpidr_hash.bits = bits[3] + bits[2] + bits[1] + bits[0];
+	pr_debug("MPIDR hash: aff0[%u] aff1[%u] aff2[%u] aff3[%u] mask[%#llx] bits[%u]\n",
+		mpidr_hash.shift_aff[0],
+		mpidr_hash.shift_aff[1],
+		mpidr_hash.shift_aff[2],
+		mpidr_hash.shift_aff[3],
+		mpidr_hash.mask,
+		mpidr_hash.bits);
+	/*
+	 * 4x is an arbitrary value used to warn on a hash table much bigger
+	 * than expected on most systems.
+	 */
+	if (mpidr_hash_size() > 4 * num_possible_cpus())
+		pr_warn("Large number of MPIDR hash buckets detected\n");
+	__flush_dcache_area(&mpidr_hash, sizeof(struct mpidr_hash));
+}
+#endif
+
 static void __init setup_processor(void)
 {
 	struct cpu_info *cpu_info;
@@ -231,6 +300,7 @@ void __init setup_arch(char **cmdline_p)
 	cpu_read_bootcpu_ops();
 #ifdef CONFIG_SMP
 	smp_init_cpus();
+	smp_build_mpidr_hash();
 #endif
 
 #ifdef CONFIG_VT
-- 
1.8.4

[PATCH RFC v3 03/12] arm64: kernel: suspend/resume registers save/restore

[Lorenzo Pieralisi]

Power management software requires the kernel to save and restore
CPU registers while going through suspend and resume operations
triggered by kernel subsystems like CPU idle and suspend to RAM.

This patch implements code that provides save and restore mechanism
for the arm v8 implementation. Memory for the context is passed as
parameter to both cpu_do_suspend and cpu_do_resume functions, and allows
the callers to implement context allocation as they deem fit.

The registers that are saved and restored correspond to the registers set
actually required by the kernel to be up and running which represents a
subset of v8 ISA.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/proc-fns.h |  3 ++
 arch/arm64/include/asm/suspend.h  | 18 ++++++++++
 arch/arm64/mm/proc.S              | 69 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 90 insertions(+)
 create mode 100644 arch/arm64/include/asm/suspend.h

diff --git a/arch/arm64/include/asm/proc-fns.h b/arch/arm64/include/asm/proc-fns.h
index 7cdf466..0c657bb 100644
--- a/arch/arm64/include/asm/proc-fns.h
+++ b/arch/arm64/include/asm/proc-fns.h
@@ -26,11 +26,14 @@
 #include <asm/page.h>
 
 struct mm_struct;
+struct cpu_suspend_ctx;
 
 extern void cpu_cache_off(void);
 extern void cpu_do_idle(void);
 extern void cpu_do_switch_mm(unsigned long pgd_phys, struct mm_struct *mm);
 extern void cpu_reset(unsigned long addr) __attribute__((noreturn));
+extern void cpu_do_suspend(struct cpu_suspend_ctx *ptr);
+extern u64 cpu_do_resume(phys_addr_t ptr, u64 idmap_ttbr);
 
 #include <asm/memory.h>
 
diff --git a/arch/arm64/include/asm/suspend.h b/arch/arm64/include/asm/suspend.h
new file mode 100644
index 0000000..a88558e
--- /dev/null
+++ b/arch/arm64/include/asm/suspend.h
@@ -0,0 +1,18 @@
+#ifndef __ASM_SUSPEND_H
+#define __ASM_SUSPEND_H
+
+#define NR_CTX_REGS 11
+
+/*
+ * struct cpu_suspend_ctx must be 16-byte aligned since it is allocated on
+ * the stack, which must be 16-byte aligned on v8
+ */
+struct cpu_suspend_ctx {
+	/*
+	 * This struct must be kept in sync with
+	 * cpu_do_{suspend/resume} in mm/proc.S
+	 */
+	u64 ctx_regs[NR_CTX_REGS];
+	u64 sp;
+} __aligned(16);
+#endif
diff --git a/arch/arm64/mm/proc.S b/arch/arm64/mm/proc.S
index 421b99f..ef91046 100644
--- a/arch/arm64/mm/proc.S
+++ b/arch/arm64/mm/proc.S
@@ -80,6 +80,75 @@ ENTRY(cpu_do_idle)
 	ret
 ENDPROC(cpu_do_idle)
 
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+/**
+ * cpu_do_suspend - save CPU registers context
+ *
+ * x0: virtual address of context pointer
+ */
+ENTRY(cpu_do_suspend)
+	mrs	x2, tpidr_el0
+	mrs	x3, tpidrro_el0
+	mrs	x4, contextidr_el1
+	mrs	x5, mair_el1
+	mrs	x6, cpacr_el1
+	mrs	x7, ttbr1_el1
+	mrs	x8, tcr_el1
+	mrs	x9, vbar_el1
+	mrs	x10, mdscr_el1
+	mrs	x11, oslsr_el1
+	mrs	x12, sctlr_el1
+	stp	x2, x3, [x0]
+	stp	x4, x5, [x0, #16]
+	stp	x6, x7, [x0, #32]
+	stp	x8, x9, [x0, #48]
+	stp	x10, x11, [x0, #64]
+	str	x12, [x0, #80]
+	ret
+ENDPROC(cpu_do_suspend)
+
+/**
+ * cpu_do_resume - restore CPU register context
+ *
+ * x0: Physical address of context pointer
+ * x1: ttbr0_el1 to be restored
+ *
+ * Returns:
+ *	sctlr_el1 value in x0
+ */
+ENTRY(cpu_do_resume)
+	/*
+	 * Invalidate local tlb entries before turning on MMU
+	 */
+	tlbi	vmalle1
+	ldp	x2, x3, [x0]
+	ldp	x4, x5, [x0, #16]
+	ldp	x6, x7, [x0, #32]
+	ldp	x8, x9, [x0, #48]
+	ldp	x10, x11, [x0, #64]
+	ldr	x12, [x0, #80]
+	msr	tpidr_el0, x2
+	msr	tpidrro_el0, x3
+	msr	contextidr_el1, x4
+	msr	mair_el1, x5
+	msr	cpacr_el1, x6
+	msr	ttbr0_el1, x1
+	msr	ttbr1_el1, x7
+	msr	tcr_el1, x8
+	msr	vbar_el1, x9
+	msr	mdscr_el1, x10
+	/*
+	 * Restore oslsr_el1 by writing oslar_el1
+	 */
+	ubfx	x11, x11, #1, #1
+	msr	oslar_el1, x11
+	mov	x0, x12
+	dsb	nsh		// Make sure local tlb invalidation completed
+	isb
+	ret
+ENDPROC(cpu_do_resume)
+#endif
+
 /*
  *	cpu_switch_mm(pgd_phys, tsk)
  *
-- 
1.8.4

[PATCH RFC v3 03/12] arm64: kernel: suspend/resume registers save/restore

[Lorenzo Pieralisi]

Power management software requires the kernel to save and restore
CPU registers while going through suspend and resume operations
triggered by kernel subsystems like CPU idle and suspend to RAM.

This patch implements code that provides save and restore mechanism
for the arm v8 implementation. Memory for the context is passed as
parameter to both cpu_do_suspend and cpu_do_resume functions, and allows
the callers to implement context allocation as they deem fit.

The registers that are saved and restored correspond to the registers set
actually required by the kernel to be up and running which represents a
subset of v8 ISA.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/proc-fns.h |  3 ++
 arch/arm64/include/asm/suspend.h  | 18 ++++++++++
 arch/arm64/mm/proc.S              | 69 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 90 insertions(+)
 create mode 100644 arch/arm64/include/asm/suspend.h

diff --git a/arch/arm64/include/asm/proc-fns.h b/arch/arm64/include/asm/proc-fns.h
index 7cdf466..0c657bb 100644
--- a/arch/arm64/include/asm/proc-fns.h
+++ b/arch/arm64/include/asm/proc-fns.h
@@ -26,11 +26,14 @@
 #include <asm/page.h>
 
 struct mm_struct;
+struct cpu_suspend_ctx;
 
 extern void cpu_cache_off(void);
 extern void cpu_do_idle(void);
 extern void cpu_do_switch_mm(unsigned long pgd_phys, struct mm_struct *mm);
 extern void cpu_reset(unsigned long addr) __attribute__((noreturn));
+extern void cpu_do_suspend(struct cpu_suspend_ctx *ptr);
+extern u64 cpu_do_resume(phys_addr_t ptr, u64 idmap_ttbr);
 
 #include <asm/memory.h>
 
diff --git a/arch/arm64/include/asm/suspend.h b/arch/arm64/include/asm/suspend.h
new file mode 100644
index 0000000..a88558e
--- /dev/null
+++ b/arch/arm64/include/asm/suspend.h
@@ -0,0 +1,18 @@
+#ifndef __ASM_SUSPEND_H
+#define __ASM_SUSPEND_H
+
+#define NR_CTX_REGS 11
+
+/*
+ * struct cpu_suspend_ctx must be 16-byte aligned since it is allocated on
+ * the stack, which must be 16-byte aligned on v8
+ */
+struct cpu_suspend_ctx {
+	/*
+	 * This struct must be kept in sync with
+	 * cpu_do_{suspend/resume} in mm/proc.S
+	 */
+	u64 ctx_regs[NR_CTX_REGS];
+	u64 sp;
+} __aligned(16);
+#endif
diff --git a/arch/arm64/mm/proc.S b/arch/arm64/mm/proc.S
index 421b99f..ef91046 100644
--- a/arch/arm64/mm/proc.S
+++ b/arch/arm64/mm/proc.S
@@ -80,6 +80,75 @@ ENTRY(cpu_do_idle)
 	ret
 ENDPROC(cpu_do_idle)
 
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+/**
+ * cpu_do_suspend - save CPU registers context
+ *
+ * x0: virtual address of context pointer
+ */
+ENTRY(cpu_do_suspend)
+	mrs	x2, tpidr_el0
+	mrs	x3, tpidrro_el0
+	mrs	x4, contextidr_el1
+	mrs	x5, mair_el1
+	mrs	x6, cpacr_el1
+	mrs	x7, ttbr1_el1
+	mrs	x8, tcr_el1
+	mrs	x9, vbar_el1
+	mrs	x10, mdscr_el1
+	mrs	x11, oslsr_el1
+	mrs	x12, sctlr_el1
+	stp	x2, x3, [x0]
+	stp	x4, x5, [x0, #16]
+	stp	x6, x7, [x0, #32]
+	stp	x8, x9, [x0, #48]
+	stp	x10, x11, [x0, #64]
+	str	x12, [x0, #80]
+	ret
+ENDPROC(cpu_do_suspend)
+
+/**
+ * cpu_do_resume - restore CPU register context
+ *
+ * x0: Physical address of context pointer
+ * x1: ttbr0_el1 to be restored
+ *
+ * Returns:
+ *	sctlr_el1 value in x0
+ */
+ENTRY(cpu_do_resume)
+	/*
+	 * Invalidate local tlb entries before turning on MMU
+	 */
+	tlbi	vmalle1
+	ldp	x2, x3, [x0]
+	ldp	x4, x5, [x0, #16]
+	ldp	x6, x7, [x0, #32]
+	ldp	x8, x9, [x0, #48]
+	ldp	x10, x11, [x0, #64]
+	ldr	x12, [x0, #80]
+	msr	tpidr_el0, x2
+	msr	tpidrro_el0, x3
+	msr	contextidr_el1, x4
+	msr	mair_el1, x5
+	msr	cpacr_el1, x6
+	msr	ttbr0_el1, x1
+	msr	ttbr1_el1, x7
+	msr	tcr_el1, x8
+	msr	vbar_el1, x9
+	msr	mdscr_el1, x10
+	/*
+	 * Restore oslsr_el1 by writing oslar_el1
+	 */
+	ubfx	x11, x11, #1, #1
+	msr	oslar_el1, x11
+	mov	x0, x12
+	dsb	nsh		// Make sure local tlb invalidation completed
+	isb
+	ret
+ENDPROC(cpu_do_resume)
+#endif
+
 /*
  *	cpu_switch_mm(pgd_phys, tsk)
  *
-- 
1.8.4

[PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Lorenzo Pieralisi]

Kernel subsystems like CPU idle and suspend to RAM require a generic
mechanism to suspend a processor, save its context and put it into
a quiescent state. The cpu_{suspend}/{resume} implementation provides
such a framework through a kernel interface allowing to save/restore
registers, flush the context to DRAM and suspend/resume to/from
low-power states where processor context may be lost.

The CPU suspend implementation relies on the suspend protocol registered
in CPU operations to carry out a suspend request after context is
saved and flushed to DRAM. The cpu_suspend interface:

int cpu_suspend(unsigned long arg);

allows to pass an opaque parameter that is handed over to the suspend CPU
operations back-end so that it can take action according to the
semantics attached to it. The arg parameter allows suspend to RAM and CPU
idle drivers to communicate to suspend protocol back-ends; it requires
standardization so that the interface can be reused seamlessly across
systems, paving the way for generic drivers.

Context memory is allocated on the stack, whose address is stashed in a
per-cpu variable to keep track of it and passed to core functions that
save/restore the registers required by the architecture.

Even though, upon successful execution, the cpu_suspend function shuts
down the suspending processor, the warm boot resume mechanism, based
on the cpu_resume function, makes the resume path operate as a
cpu_suspend function return, so that cpu_suspend can be treated as a C
function by the caller, which simplifies coding the PM drivers that rely
on the cpu_suspend API.

Upon context save, the minimal amount of memory is flushed to DRAM so
that it can be retrieved when the MMU is off and caches are not searched.

The suspend CPU operation, depending on the required operations (eg CPU vs
Cluster shutdown) is in charge of flushing the cache hierarchy either
implicitly (by calling firmware implementations like PSCI) or explicitly
by executing the required cache maintainance functions.

Debug exceptions are disabled during cpu_{suspend}/{resume} operations
so that debug registers can be saved and restored properly preventing
preemption from debug agents enabled in the kernel.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/cpu_ops.h |   6 ++
 arch/arm64/include/asm/suspend.h |   9 ++
 arch/arm64/kernel/asm-offsets.c  |  11 +++
 arch/arm64/kernel/sleep.S        | 184 +++++++++++++++++++++++++++++++++++++++
 arch/arm64/kernel/suspend.c      | 109 +++++++++++++++++++++++
 5 files changed, 319 insertions(+)
 create mode 100644 arch/arm64/kernel/sleep.S
 create mode 100644 arch/arm64/kernel/suspend.c

diff --git a/arch/arm64/include/asm/cpu_ops.h b/arch/arm64/include/asm/cpu_ops.h
index c4cdb5e..1524130 100644
--- a/arch/arm64/include/asm/cpu_ops.h
+++ b/arch/arm64/include/asm/cpu_ops.h
@@ -39,6 +39,9 @@ struct device_node;
  * 		from the cpu to be killed.
  * @cpu_die:	Makes a cpu leave the kernel. Must not fail. Called from the
  *		cpu being killed.
+ * @cpu_suspend: Suspends a cpu and saves the required context. May fail owing
+ *               to wrong parameters or error conditions. Called from the
+ *               CPU being suspended. Must be called with IRQs disabled.
  */
 struct cpu_operations {
 	const char	*name;
@@ -50,6 +53,9 @@ struct cpu_operations {
 	int		(*cpu_disable)(unsigned int cpu);
 	void		(*cpu_die)(unsigned int cpu);
 #endif
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+	int		(*cpu_suspend)(unsigned long);
+#endif
 };
 
 extern const struct cpu_operations *cpu_ops[NR_CPUS];
diff --git a/arch/arm64/include/asm/suspend.h b/arch/arm64/include/asm/suspend.h
index a88558e..e9c149c 100644
--- a/arch/arm64/include/asm/suspend.h
+++ b/arch/arm64/include/asm/suspend.h
@@ -15,4 +15,13 @@ struct cpu_suspend_ctx {
 	u64 ctx_regs[NR_CTX_REGS];
 	u64 sp;
 } __aligned(16);
+
+struct sleep_save_sp {
+	phys_addr_t *save_ptr_stash;
+	phys_addr_t save_ptr_stash_phys;
+};
+
+extern void cpu_resume(void);
+extern int cpu_suspend(unsigned long);
+
 #endif
diff --git a/arch/arm64/kernel/asm-offsets.c b/arch/arm64/kernel/asm-offsets.c
index 666e231..646f888 100644
--- a/arch/arm64/kernel/asm-offsets.c
+++ b/arch/arm64/kernel/asm-offsets.c
@@ -25,6 +25,8 @@
 #include <asm/thread_info.h>
 #include <asm/memory.h>
 #include <asm/cputable.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
 #include <asm/vdso_datapage.h>
 #include <linux/kbuild.h>
 
@@ -138,5 +140,14 @@ int main(void)
   DEFINE(KVM_VTTBR,		offsetof(struct kvm, arch.vttbr));
   DEFINE(KVM_VGIC_VCTRL,	offsetof(struct kvm, arch.vgic.vctrl_base));
 #endif
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+  DEFINE(CPU_SUSPEND_SZ,	sizeof(struct cpu_suspend_ctx));
+  DEFINE(CPU_CTX_SP,		offsetof(struct cpu_suspend_ctx, sp));
+  DEFINE(MPIDR_HASH_MASK,	offsetof(struct mpidr_hash, mask));
+  DEFINE(MPIDR_HASH_SHIFTS,	offsetof(struct mpidr_hash, shift_aff));
+  DEFINE(SLEEP_SAVE_SP_SZ,	sizeof(struct sleep_save_sp));
+  DEFINE(SLEEP_SAVE_SP_PHYS,	offsetof(struct sleep_save_sp, save_ptr_stash_phys));
+  DEFINE(SLEEP_SAVE_SP_VIRT,	offsetof(struct sleep_save_sp, save_ptr_stash));
+#endif
   return 0;
 }
diff --git a/arch/arm64/kernel/sleep.S b/arch/arm64/kernel/sleep.S
new file mode 100644
index 0000000..b192572
--- /dev/null
+++ b/arch/arm64/kernel/sleep.S
@@ -0,0 +1,184 @@
+#include <linux/errno.h>
+#include <linux/linkage.h>
+#include <asm/asm-offsets.h>
+#include <asm/assembler.h>
+
+	.text
+/*
+ * Implementation of MPIDR_EL1 hash algorithm through shifting
+ * and OR'ing.
+ *
+ * @dst: register containing hash result
+ * @rs0: register containing affinity level 0 bit shift
+ * @rs1: register containing affinity level 1 bit shift
+ * @rs2: register containing affinity level 2 bit shift
+ * @rs3: register containing affinity level 3 bit shift
+ * @mpidr: register containing MPIDR_EL1 value
+ * @mask: register containing MPIDR mask
+ *
+ * Pseudo C-code:
+ *
+ *u32 dst;
+ *
+ *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 rs3, u64 mpidr, u64 mask) {
+ *	u32 aff0, aff1, aff2, aff3;
+ *	u64 mpidr_masked = mpidr & mask;
+ *	aff0 = mpidr_masked & 0xff;
+ *	aff1 = mpidr_masked & 0xff00;
+ *	aff2 = mpidr_masked & 0xff0000;
+ *	aff2 = mpidr_masked & 0xff00000000;
+ *	dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2 | aff3 >> rs3);
+ *}
+ * Input registers: rs0, rs1, rs2, rs3, mpidr, mask
+ * Output register: dst
+ * Note: input and output registers must be disjoint register sets
+         (eg: a macro instance with mpidr = x1 and dst = x1 is invalid)
+ */
+	.macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask
+	and	\mpidr, \mpidr, \mask		// mask out MPIDR bits
+	and	\dst, \mpidr, #0xff		// mask=aff0
+	lsr	\dst ,\dst, \rs0		// dst=aff0>>rs0
+	and	\mask, \mpidr, #0xff00		// mask = aff1
+	lsr	\mask ,\mask, \rs1
+	orr	\dst, \dst, \mask		// dst|=(aff1>>rs1)
+	and	\mask, \mpidr, #0xff0000	// mask = aff2
+	lsr	\mask ,\mask, \rs2
+	orr	\dst, \dst, \mask		// dst|=(aff2>>rs2)
+	and	\mask, \mpidr, #0xff00000000	// mask = aff3
+	lsr	\mask ,\mask, \rs3
+	orr	\dst, \dst, \mask		// dst|=(aff3>>rs3)
+	.endm
+/*
+ * Save CPU state for a suspend.  This saves callee registers, and allocates
+ * space on the kernel stack to save the CPU specific registers + some
+ * other data for resume.
+ *
+ *  x0 = suspend finisher argument
+ */
+ENTRY(__cpu_suspend)
+	stp	x29, lr, [sp, #-96]!
+	stp	x19, x20, [sp,#16]
+	stp	x21, x22, [sp,#32]
+	stp	x23, x24, [sp,#48]
+	stp	x25, x26, [sp,#64]
+	stp	x27, x28, [sp,#80]
+	mov	x2, sp
+	sub	sp, sp, #CPU_SUSPEND_SZ	// allocate cpu_suspend_ctx
+	mov	x1, sp
+	/*
+	 * x1 now points to struct cpu_suspend_ctx allocated on the stack
+	 */
+	str	x2, [x1, #CPU_CTX_SP]
+	ldr	x2, =sleep_save_sp
+	ldr	x2, [x2, #SLEEP_SAVE_SP_VIRT]
+#ifdef CONFIG_SMP
+	mrs	x7, mpidr_el1
+	ldr	x9, =mpidr_hash
+	ldr	x10, [x9, #MPIDR_HASH_MASK]
+	/*
+	 * Following code relies on the struct mpidr_hash
+	 * members size.
+	 */
+	ldp	w3, w4, [x9, #MPIDR_HASH_SHIFTS]
+	ldp	w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
+	compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
+	add	x2, x2, x8, lsl #3
+#endif
+	bl	__cpu_suspend_finisher
+        /*
+	 * Never gets here, unless suspend fails.
+	 * Successful cpu_suspend should return from cpu_resume, returning
+	 * through this code path is considered an error
+	 * If the return value is set to 0 force x0 = -EOPNOTSUPP
+	 * to make sure a proper error condition is propagated
+	 */
+	cmp	x0, #0
+	mov	x3, #-EOPNOTSUPP
+	csel	x0, x3, x0, eq
+	add	sp, sp, #CPU_SUSPEND_SZ	// rewind stack pointer
+	ldp	x19, x20, [sp, #16]
+	ldp	x21, x22, [sp, #32]
+	ldp	x23, x24, [sp, #48]
+	ldp	x25, x26, [sp, #64]
+	ldp	x27, x28, [sp, #80]
+	ldp	x29, lr, [sp], #96
+	ret
+ENDPROC(__cpu_suspend)
+	.ltorg
+
+/*
+ * x0 must contain the sctlr value retrieved from restored context
+ */
+ENTRY(cpu_resume_mmu)
+	ldr	x3, =cpu_resume_after_mmu
+	msr	sctlr_el1, x0		// restore sctlr_el1
+	isb
+	br	x3			// global jump to virtual address
+ENDPROC(cpu_resume_mmu)
+cpu_resume_after_mmu:
+	mov	x0, #0			// return zero on success
+	ldp	x19, x20, [sp, #16]
+	ldp	x21, x22, [sp, #32]
+	ldp	x23, x24, [sp, #48]
+	ldp	x25, x26, [sp, #64]
+	ldp	x27, x28, [sp, #80]
+	ldp	x29, lr, [sp], #96
+	ret
+ENDPROC(cpu_resume_after_mmu)
+
+	.data
+ENTRY(cpu_resume)
+	bl	el2_setup		// if in EL2 drop to EL1 cleanly
+#ifdef CONFIG_SMP
+	mrs	x1, mpidr_el1
+	adr	x4, mpidr_hash_ptr
+	ldr	x5, [x4]
+	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
+        /* retrieve mpidr_hash members to compute the hash */
+	ldr	x2, [x8, #MPIDR_HASH_MASK]
+	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
+	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
+	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
+        /* x7 contains hash index, let's use it to grab context pointer */
+#else
+	mov	x7, xzr
+#endif
+	adr	x0, sleep_save_sp
+	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
+	ldr	x0, [x0, x7, lsl #3]
+	/* load sp from context */
+	ldr	x2, [x0, #CPU_CTX_SP]
+	adr	x1, sleep_idmap_phys
+	/* load physical address of identity map page table in x1 */
+	ldr	x1, [x1]
+	mov	sp, x2
+	/*
+	 * cpu_do_resume expects x0 to contain context physical address
+	 * pointer and x1 to contain physical address of 1:1 page tables
+	 */
+	bl	cpu_do_resume		// PC relative jump, MMU off
+	b	cpu_resume_mmu		// Resume MMU, never returns
+ENDPROC(cpu_resume)
+
+	.align 3
+mpidr_hash_ptr:
+	/*
+	 * offset of mpidr_hash symbol from current location
+	 * used to obtain run-time mpidr_hash address with MMU off
+         */
+	.quad	mpidr_hash - .
+/*
+ * physical address of identity mapped page tables
+ */
+	.type	sleep_idmap_phys, #object
+ENTRY(sleep_idmap_phys)
+	.quad	0
+/*
+ * struct sleep_save_sp {
+ *	phys_addr_t *save_ptr_stash;
+ *	phys_addr_t save_ptr_stash_phys;
+ * };
+ */
+	.type	sleep_save_sp, #object
+ENTRY(sleep_save_sp)
+	.space	SLEEP_SAVE_SP_SZ	// struct sleep_save_sp
diff --git a/arch/arm64/kernel/suspend.c b/arch/arm64/kernel/suspend.c
new file mode 100644
index 0000000..e074b1c
--- /dev/null
+++ b/arch/arm64/kernel/suspend.c
@@ -0,0 +1,109 @@
+#include <linux/slab.h>
+#include <asm/cacheflush.h>
+#include <asm/cpu_ops.h>
+#include <asm/debug-monitors.h>
+#include <asm/pgtable.h>
+#include <asm/memory.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
+#include <asm/tlbflush.h>
+
+extern int __cpu_suspend(unsigned long);
+/*
+ * This is called by __cpu_suspend() to save the state, and do whatever
+ * flushing is required to ensure that when the CPU goes to sleep we have
+ * the necessary data available when the caches are not searched.
+ *
+ * @arg: Argument to pass to suspend operations
+ * @ptr: CPU context virtual address
+ * @save_ptr: address of the location where the context physical address
+ *            must be saved
+ */
+int __cpu_suspend_finisher(unsigned long arg, struct cpu_suspend_ctx *ptr,
+			   phys_addr_t *save_ptr)
+{
+	int cpu = smp_processor_id();
+
+	*save_ptr = virt_to_phys(ptr);
+
+	cpu_do_suspend(ptr);
+	/*
+	 * Only flush the context that must be retrieved with the MMU
+	 * off. VA primitives ensure the flush is applied to all
+	 * cache levels so context is pushed to DRAM.
+	 */
+	__flush_dcache_area(ptr, sizeof(*ptr));
+	__flush_dcache_area(save_ptr, sizeof(*save_ptr));
+
+	return cpu_ops[cpu]->cpu_suspend(arg);
+}
+
+/**
+ * cpu_suspend
+ *
+ * @arg: argument to pass to the finisher function
+ */
+int cpu_suspend(unsigned long arg)
+{
+	struct mm_struct *mm = current->active_mm;
+	int ret, cpu = smp_processor_id();
+	unsigned long flags;
+
+	/*
+	 * If cpu_ops have not been registered or suspend
+	 * has not been initialized, cpu_suspend call fails early.
+	 */
+	if (!cpu_ops[cpu] || !cpu_ops[cpu]->cpu_suspend)
+		return -EOPNOTSUPP;
+
+	/*
+	 * From this point debug exceptions are disabled to prevent
+	 * updates to mdscr register (saved and restored along with
+	 * general purpose registers) from kernel debuggers.
+	 */
+	local_dbg_save(flags);
+
+	/*
+	 * mm context saved on the stack, it will be restored when
+	 * the cpu comes out of reset through the identity mapped
+	 * page tables, so that the thread address space is properly
+	 * set-up on function return.
+	 */
+	ret = __cpu_suspend(arg);
+	if (ret == 0) {
+		cpu_switch_mm(mm->pgd, mm);
+		flush_tlb_all();
+	}
+
+	/*
+	 * Restore pstate flags. OS lock and mdscr have been already
+	 * restored, so from this point onwards, debugging is fully
+	 * renabled if it was enabled when core started shutdown.
+	 */
+	local_dbg_restore(flags);
+
+	return ret;
+}
+
+extern struct sleep_save_sp sleep_save_sp;
+extern phys_addr_t sleep_idmap_phys;
+
+static int cpu_suspend_init(void)
+{
+	void *ctx_ptr;
+
+	/* ctx_ptr is an array of physical addresses */
+	ctx_ptr = kcalloc(mpidr_hash_size(), sizeof(phys_addr_t), GFP_KERNEL);
+
+	if (WARN_ON(!ctx_ptr))
+		return -ENOMEM;
+
+	sleep_save_sp.save_ptr_stash = ctx_ptr;
+	sleep_save_sp.save_ptr_stash_phys = virt_to_phys(ctx_ptr);
+	sleep_idmap_phys = virt_to_phys(idmap_pg_dir);
+	__flush_dcache_area(&sleep_save_sp, sizeof(struct sleep_save_sp));
+	__flush_dcache_area(&sleep_idmap_phys, sizeof(sleep_idmap_phys));
+
+	return 0;
+}
+early_initcall(cpu_suspend_init);
-- 
1.8.4

[PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Lorenzo Pieralisi]

Kernel subsystems like CPU idle and suspend to RAM require a generic
mechanism to suspend a processor, save its context and put it into
a quiescent state. The cpu_{suspend}/{resume} implementation provides
such a framework through a kernel interface allowing to save/restore
registers, flush the context to DRAM and suspend/resume to/from
low-power states where processor context may be lost.

The CPU suspend implementation relies on the suspend protocol registered
in CPU operations to carry out a suspend request after context is
saved and flushed to DRAM. The cpu_suspend interface:

int cpu_suspend(unsigned long arg);

allows to pass an opaque parameter that is handed over to the suspend CPU
operations back-end so that it can take action according to the
semantics attached to it. The arg parameter allows suspend to RAM and CPU
idle drivers to communicate to suspend protocol back-ends; it requires
standardization so that the interface can be reused seamlessly across
systems, paving the way for generic drivers.

Context memory is allocated on the stack, whose address is stashed in a
per-cpu variable to keep track of it and passed to core functions that
save/restore the registers required by the architecture.

Even though, upon successful execution, the cpu_suspend function shuts
down the suspending processor, the warm boot resume mechanism, based
on the cpu_resume function, makes the resume path operate as a
cpu_suspend function return, so that cpu_suspend can be treated as a C
function by the caller, which simplifies coding the PM drivers that rely
on the cpu_suspend API.

Upon context save, the minimal amount of memory is flushed to DRAM so
that it can be retrieved when the MMU is off and caches are not searched.

The suspend CPU operation, depending on the required operations (eg CPU vs
Cluster shutdown) is in charge of flushing the cache hierarchy either
implicitly (by calling firmware implementations like PSCI) or explicitly
by executing the required cache maintainance functions.

Debug exceptions are disabled during cpu_{suspend}/{resume} operations
so that debug registers can be saved and restored properly preventing
preemption from debug agents enabled in the kernel.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/include/asm/cpu_ops.h |   6 ++
 arch/arm64/include/asm/suspend.h |   9 ++
 arch/arm64/kernel/asm-offsets.c  |  11 +++
 arch/arm64/kernel/sleep.S        | 184 +++++++++++++++++++++++++++++++++++++++
 arch/arm64/kernel/suspend.c      | 109 +++++++++++++++++++++++
 5 files changed, 319 insertions(+)
 create mode 100644 arch/arm64/kernel/sleep.S
 create mode 100644 arch/arm64/kernel/suspend.c

diff --git a/arch/arm64/include/asm/cpu_ops.h b/arch/arm64/include/asm/cpu_ops.h
index c4cdb5e..1524130 100644
--- a/arch/arm64/include/asm/cpu_ops.h
+++ b/arch/arm64/include/asm/cpu_ops.h
@@ -39,6 +39,9 @@ struct device_node;
  * 		from the cpu to be killed.
  * @cpu_die:	Makes a cpu leave the kernel. Must not fail. Called from the
  *		cpu being killed.
+ * @cpu_suspend: Suspends a cpu and saves the required context. May fail owing
+ *               to wrong parameters or error conditions. Called from the
+ *               CPU being suspended. Must be called with IRQs disabled.
  */
 struct cpu_operations {
 	const char	*name;
@@ -50,6 +53,9 @@ struct cpu_operations {
 	int		(*cpu_disable)(unsigned int cpu);
 	void		(*cpu_die)(unsigned int cpu);
 #endif
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+	int		(*cpu_suspend)(unsigned long);
+#endif
 };
 
 extern const struct cpu_operations *cpu_ops[NR_CPUS];
diff --git a/arch/arm64/include/asm/suspend.h b/arch/arm64/include/asm/suspend.h
index a88558e..e9c149c 100644
--- a/arch/arm64/include/asm/suspend.h
+++ b/arch/arm64/include/asm/suspend.h
@@ -15,4 +15,13 @@ struct cpu_suspend_ctx {
 	u64 ctx_regs[NR_CTX_REGS];
 	u64 sp;
 } __aligned(16);
+
+struct sleep_save_sp {
+	phys_addr_t *save_ptr_stash;
+	phys_addr_t save_ptr_stash_phys;
+};
+
+extern void cpu_resume(void);
+extern int cpu_suspend(unsigned long);
+
 #endif
diff --git a/arch/arm64/kernel/asm-offsets.c b/arch/arm64/kernel/asm-offsets.c
index 666e231..646f888 100644
--- a/arch/arm64/kernel/asm-offsets.c
+++ b/arch/arm64/kernel/asm-offsets.c
@@ -25,6 +25,8 @@
 #include <asm/thread_info.h>
 #include <asm/memory.h>
 #include <asm/cputable.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
 #include <asm/vdso_datapage.h>
 #include <linux/kbuild.h>
 
@@ -138,5 +140,14 @@ int main(void)
   DEFINE(KVM_VTTBR,		offsetof(struct kvm, arch.vttbr));
   DEFINE(KVM_VGIC_VCTRL,	offsetof(struct kvm, arch.vgic.vctrl_base));
 #endif
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+  DEFINE(CPU_SUSPEND_SZ,	sizeof(struct cpu_suspend_ctx));
+  DEFINE(CPU_CTX_SP,		offsetof(struct cpu_suspend_ctx, sp));
+  DEFINE(MPIDR_HASH_MASK,	offsetof(struct mpidr_hash, mask));
+  DEFINE(MPIDR_HASH_SHIFTS,	offsetof(struct mpidr_hash, shift_aff));
+  DEFINE(SLEEP_SAVE_SP_SZ,	sizeof(struct sleep_save_sp));
+  DEFINE(SLEEP_SAVE_SP_PHYS,	offsetof(struct sleep_save_sp, save_ptr_stash_phys));
+  DEFINE(SLEEP_SAVE_SP_VIRT,	offsetof(struct sleep_save_sp, save_ptr_stash));
+#endif
   return 0;
 }
diff --git a/arch/arm64/kernel/sleep.S b/arch/arm64/kernel/sleep.S
new file mode 100644
index 0000000..b192572
--- /dev/null
+++ b/arch/arm64/kernel/sleep.S
@@ -0,0 +1,184 @@
+#include <linux/errno.h>
+#include <linux/linkage.h>
+#include <asm/asm-offsets.h>
+#include <asm/assembler.h>
+
+	.text
+/*
+ * Implementation of MPIDR_EL1 hash algorithm through shifting
+ * and OR'ing.
+ *
+ * @dst: register containing hash result
+ * @rs0: register containing affinity level 0 bit shift
+ * @rs1: register containing affinity level 1 bit shift
+ * @rs2: register containing affinity level 2 bit shift
+ * @rs3: register containing affinity level 3 bit shift
+ * @mpidr: register containing MPIDR_EL1 value
+ * @mask: register containing MPIDR mask
+ *
+ * Pseudo C-code:
+ *
+ *u32 dst;
+ *
+ *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 rs3, u64 mpidr, u64 mask) {
+ *	u32 aff0, aff1, aff2, aff3;
+ *	u64 mpidr_masked = mpidr & mask;
+ *	aff0 = mpidr_masked & 0xff;
+ *	aff1 = mpidr_masked & 0xff00;
+ *	aff2 = mpidr_masked & 0xff0000;
+ *	aff2 = mpidr_masked & 0xff00000000;
+ *	dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2 | aff3 >> rs3);
+ *}
+ * Input registers: rs0, rs1, rs2, rs3, mpidr, mask
+ * Output register: dst
+ * Note: input and output registers must be disjoint register sets
+         (eg: a macro instance with mpidr = x1 and dst = x1 is invalid)
+ */
+	.macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask
+	and	\mpidr, \mpidr, \mask		// mask out MPIDR bits
+	and	\dst, \mpidr, #0xff		// mask=aff0
+	lsr	\dst ,\dst, \rs0		// dst=aff0>>rs0
+	and	\mask, \mpidr, #0xff00		// mask = aff1
+	lsr	\mask ,\mask, \rs1
+	orr	\dst, \dst, \mask		// dst|=(aff1>>rs1)
+	and	\mask, \mpidr, #0xff0000	// mask = aff2
+	lsr	\mask ,\mask, \rs2
+	orr	\dst, \dst, \mask		// dst|=(aff2>>rs2)
+	and	\mask, \mpidr, #0xff00000000	// mask = aff3
+	lsr	\mask ,\mask, \rs3
+	orr	\dst, \dst, \mask		// dst|=(aff3>>rs3)
+	.endm
+/*
+ * Save CPU state for a suspend.  This saves callee registers, and allocates
+ * space on the kernel stack to save the CPU specific registers + some
+ * other data for resume.
+ *
+ *  x0 = suspend finisher argument
+ */
+ENTRY(__cpu_suspend)
+	stp	x29, lr, [sp, #-96]!
+	stp	x19, x20, [sp,#16]
+	stp	x21, x22, [sp,#32]
+	stp	x23, x24, [sp,#48]
+	stp	x25, x26, [sp,#64]
+	stp	x27, x28, [sp,#80]
+	mov	x2, sp
+	sub	sp, sp, #CPU_SUSPEND_SZ	// allocate cpu_suspend_ctx
+	mov	x1, sp
+	/*
+	 * x1 now points to struct cpu_suspend_ctx allocated on the stack
+	 */
+	str	x2, [x1, #CPU_CTX_SP]
+	ldr	x2, =sleep_save_sp
+	ldr	x2, [x2, #SLEEP_SAVE_SP_VIRT]
+#ifdef CONFIG_SMP
+	mrs	x7, mpidr_el1
+	ldr	x9, =mpidr_hash
+	ldr	x10, [x9, #MPIDR_HASH_MASK]
+	/*
+	 * Following code relies on the struct mpidr_hash
+	 * members size.
+	 */
+	ldp	w3, w4, [x9, #MPIDR_HASH_SHIFTS]
+	ldp	w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
+	compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
+	add	x2, x2, x8, lsl #3
+#endif
+	bl	__cpu_suspend_finisher
+        /*
+	 * Never gets here, unless suspend fails.
+	 * Successful cpu_suspend should return from cpu_resume, returning
+	 * through this code path is considered an error
+	 * If the return value is set to 0 force x0 = -EOPNOTSUPP
+	 * to make sure a proper error condition is propagated
+	 */
+	cmp	x0, #0
+	mov	x3, #-EOPNOTSUPP
+	csel	x0, x3, x0, eq
+	add	sp, sp, #CPU_SUSPEND_SZ	// rewind stack pointer
+	ldp	x19, x20, [sp, #16]
+	ldp	x21, x22, [sp, #32]
+	ldp	x23, x24, [sp, #48]
+	ldp	x25, x26, [sp, #64]
+	ldp	x27, x28, [sp, #80]
+	ldp	x29, lr, [sp], #96
+	ret
+ENDPROC(__cpu_suspend)
+	.ltorg
+
+/*
+ * x0 must contain the sctlr value retrieved from restored context
+ */
+ENTRY(cpu_resume_mmu)
+	ldr	x3, =cpu_resume_after_mmu
+	msr	sctlr_el1, x0		// restore sctlr_el1
+	isb
+	br	x3			// global jump to virtual address
+ENDPROC(cpu_resume_mmu)
+cpu_resume_after_mmu:
+	mov	x0, #0			// return zero on success
+	ldp	x19, x20, [sp, #16]
+	ldp	x21, x22, [sp, #32]
+	ldp	x23, x24, [sp, #48]
+	ldp	x25, x26, [sp, #64]
+	ldp	x27, x28, [sp, #80]
+	ldp	x29, lr, [sp], #96
+	ret
+ENDPROC(cpu_resume_after_mmu)
+
+	.data
+ENTRY(cpu_resume)
+	bl	el2_setup		// if in EL2 drop to EL1 cleanly
+#ifdef CONFIG_SMP
+	mrs	x1, mpidr_el1
+	adr	x4, mpidr_hash_ptr
+	ldr	x5, [x4]
+	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
+        /* retrieve mpidr_hash members to compute the hash */
+	ldr	x2, [x8, #MPIDR_HASH_MASK]
+	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
+	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
+	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
+        /* x7 contains hash index, let's use it to grab context pointer */
+#else
+	mov	x7, xzr
+#endif
+	adr	x0, sleep_save_sp
+	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
+	ldr	x0, [x0, x7, lsl #3]
+	/* load sp from context */
+	ldr	x2, [x0, #CPU_CTX_SP]
+	adr	x1, sleep_idmap_phys
+	/* load physical address of identity map page table in x1 */
+	ldr	x1, [x1]
+	mov	sp, x2
+	/*
+	 * cpu_do_resume expects x0 to contain context physical address
+	 * pointer and x1 to contain physical address of 1:1 page tables
+	 */
+	bl	cpu_do_resume		// PC relative jump, MMU off
+	b	cpu_resume_mmu		// Resume MMU, never returns
+ENDPROC(cpu_resume)
+
+	.align 3
+mpidr_hash_ptr:
+	/*
+	 * offset of mpidr_hash symbol from current location
+	 * used to obtain run-time mpidr_hash address with MMU off
+         */
+	.quad	mpidr_hash - .
+/*
+ * physical address of identity mapped page tables
+ */
+	.type	sleep_idmap_phys, #object
+ENTRY(sleep_idmap_phys)
+	.quad	0
+/*
+ * struct sleep_save_sp {
+ *	phys_addr_t *save_ptr_stash;
+ *	phys_addr_t save_ptr_stash_phys;
+ * };
+ */
+	.type	sleep_save_sp, #object
+ENTRY(sleep_save_sp)
+	.space	SLEEP_SAVE_SP_SZ	// struct sleep_save_sp
diff --git a/arch/arm64/kernel/suspend.c b/arch/arm64/kernel/suspend.c
new file mode 100644
index 0000000..e074b1c
--- /dev/null
+++ b/arch/arm64/kernel/suspend.c
@@ -0,0 +1,109 @@
+#include <linux/slab.h>
+#include <asm/cacheflush.h>
+#include <asm/cpu_ops.h>
+#include <asm/debug-monitors.h>
+#include <asm/pgtable.h>
+#include <asm/memory.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
+#include <asm/tlbflush.h>
+
+extern int __cpu_suspend(unsigned long);
+/*
+ * This is called by __cpu_suspend() to save the state, and do whatever
+ * flushing is required to ensure that when the CPU goes to sleep we have
+ * the necessary data available when the caches are not searched.
+ *
+ * @arg: Argument to pass to suspend operations
+ * @ptr: CPU context virtual address
+ * @save_ptr: address of the location where the context physical address
+ *            must be saved
+ */
+int __cpu_suspend_finisher(unsigned long arg, struct cpu_suspend_ctx *ptr,
+			   phys_addr_t *save_ptr)
+{
+	int cpu = smp_processor_id();
+
+	*save_ptr = virt_to_phys(ptr);
+
+	cpu_do_suspend(ptr);
+	/*
+	 * Only flush the context that must be retrieved with the MMU
+	 * off. VA primitives ensure the flush is applied to all
+	 * cache levels so context is pushed to DRAM.
+	 */
+	__flush_dcache_area(ptr, sizeof(*ptr));
+	__flush_dcache_area(save_ptr, sizeof(*save_ptr));
+
+	return cpu_ops[cpu]->cpu_suspend(arg);
+}
+
+/**
+ * cpu_suspend
+ *
+ * @arg: argument to pass to the finisher function
+ */
+int cpu_suspend(unsigned long arg)
+{
+	struct mm_struct *mm = current->active_mm;
+	int ret, cpu = smp_processor_id();
+	unsigned long flags;
+
+	/*
+	 * If cpu_ops have not been registered or suspend
+	 * has not been initialized, cpu_suspend call fails early.
+	 */
+	if (!cpu_ops[cpu] || !cpu_ops[cpu]->cpu_suspend)
+		return -EOPNOTSUPP;
+
+	/*
+	 * From this point debug exceptions are disabled to prevent
+	 * updates to mdscr register (saved and restored along with
+	 * general purpose registers) from kernel debuggers.
+	 */
+	local_dbg_save(flags);
+
+	/*
+	 * mm context saved on the stack, it will be restored when
+	 * the cpu comes out of reset through the identity mapped
+	 * page tables, so that the thread address space is properly
+	 * set-up on function return.
+	 */
+	ret = __cpu_suspend(arg);
+	if (ret == 0) {
+		cpu_switch_mm(mm->pgd, mm);
+		flush_tlb_all();
+	}
+
+	/*
+	 * Restore pstate flags. OS lock and mdscr have been already
+	 * restored, so from this point onwards, debugging is fully
+	 * renabled if it was enabled when core started shutdown.
+	 */
+	local_dbg_restore(flags);
+
+	return ret;
+}
+
+extern struct sleep_save_sp sleep_save_sp;
+extern phys_addr_t sleep_idmap_phys;
+
+static int cpu_suspend_init(void)
+{
+	void *ctx_ptr;
+
+	/* ctx_ptr is an array of physical addresses */
+	ctx_ptr = kcalloc(mpidr_hash_size(), sizeof(phys_addr_t), GFP_KERNEL);
+
+	if (WARN_ON(!ctx_ptr))
+		return -ENOMEM;
+
+	sleep_save_sp.save_ptr_stash = ctx_ptr;
+	sleep_save_sp.save_ptr_stash_phys = virt_to_phys(ctx_ptr);
+	sleep_idmap_phys = virt_to_phys(idmap_pg_dir);
+	__flush_dcache_area(&sleep_save_sp, sizeof(struct sleep_save_sp));
+	__flush_dcache_area(&sleep_idmap_phys, sizeof(sleep_idmap_phys));
+
+	return 0;
+}
+early_initcall(cpu_suspend_init);
-- 
1.8.4

[PATCH RFC v3 05/12] arm64: kernel: implement fpsimd CPU PM notifier

[Lorenzo Pieralisi]

When a CPU enters a low power state, its FP register content is lost.
This patch adds a notifier to save the FP context on CPU shutdown
and restore it on CPU resume. The context is saved and restored only
if the suspending thread is not a kernel thread, mirroring the current
context switch behaviour.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/fpsimd.c | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/arch/arm64/kernel/fpsimd.c b/arch/arm64/kernel/fpsimd.c
index bb785d2..4aef42a 100644
--- a/arch/arm64/kernel/fpsimd.c
+++ b/arch/arm64/kernel/fpsimd.c
@@ -17,6 +17,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/cpu_pm.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/sched.h>
@@ -113,6 +114,39 @@ EXPORT_SYMBOL(kernel_neon_end);
 
 #endif /* CONFIG_KERNEL_MODE_NEON */
 
+#ifdef CONFIG_CPU_PM
+static int fpsimd_cpu_pm_notifier(struct notifier_block *self,
+				  unsigned long cmd, void *v)
+{
+	switch (cmd) {
+	case CPU_PM_ENTER:
+		if (current->mm)
+			fpsimd_save_state(&current->thread.fpsimd_state);
+		break;
+	case CPU_PM_EXIT:
+		if (current->mm)
+			fpsimd_load_state(&current->thread.fpsimd_state);
+		break;
+	case CPU_PM_ENTER_FAILED:
+	default:
+		return NOTIFY_DONE;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block fpsimd_cpu_pm_notifier_block = {
+	.notifier_call = fpsimd_cpu_pm_notifier,
+};
+
+static void fpsimd_pm_init(void)
+{
+	cpu_pm_register_notifier(&fpsimd_cpu_pm_notifier_block);
+}
+
+#else
+static inline void fpsimd_pm_init(void) { }
+#endif /* CONFIG_CPU_PM */
+
 /*
  * FP/SIMD support code initialisation.
  */
@@ -131,6 +165,8 @@ static int __init fpsimd_init(void)
 	else
 		elf_hwcap |= HWCAP_ASIMD;
 
+	fpsimd_pm_init();
+
 	return 0;
 }
 late_initcall(fpsimd_init);
-- 
1.8.4

[PATCH RFC v3 05/12] arm64: kernel: implement fpsimd CPU PM notifier

[Lorenzo Pieralisi]

When a CPU enters a low power state, its FP register content is lost.
This patch adds a notifier to save the FP context on CPU shutdown
and restore it on CPU resume. The context is saved and restored only
if the suspending thread is not a kernel thread, mirroring the current
context switch behaviour.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/fpsimd.c | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/arch/arm64/kernel/fpsimd.c b/arch/arm64/kernel/fpsimd.c
index bb785d2..4aef42a 100644
--- a/arch/arm64/kernel/fpsimd.c
+++ b/arch/arm64/kernel/fpsimd.c
@@ -17,6 +17,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/cpu_pm.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/sched.h>
@@ -113,6 +114,39 @@ EXPORT_SYMBOL(kernel_neon_end);
 
 #endif /* CONFIG_KERNEL_MODE_NEON */
 
+#ifdef CONFIG_CPU_PM
+static int fpsimd_cpu_pm_notifier(struct notifier_block *self,
+				  unsigned long cmd, void *v)
+{
+	switch (cmd) {
+	case CPU_PM_ENTER:
+		if (current->mm)
+			fpsimd_save_state(&current->thread.fpsimd_state);
+		break;
+	case CPU_PM_EXIT:
+		if (current->mm)
+			fpsimd_load_state(&current->thread.fpsimd_state);
+		break;
+	case CPU_PM_ENTER_FAILED:
+	default:
+		return NOTIFY_DONE;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block fpsimd_cpu_pm_notifier_block = {
+	.notifier_call = fpsimd_cpu_pm_notifier,
+};
+
+static void fpsimd_pm_init(void)
+{
+	cpu_pm_register_notifier(&fpsimd_cpu_pm_notifier_block);
+}
+
+#else
+static inline void fpsimd_pm_init(void) { }
+#endif /* CONFIG_CPU_PM */
+
 /*
  * FP/SIMD support code initialisation.
  */
@@ -131,6 +165,8 @@ static int __init fpsimd_init(void)
 	else
 		elf_hwcap |= HWCAP_ASIMD;
 
+	fpsimd_pm_init();
+
 	return 0;
 }
 late_initcall(fpsimd_init);
-- 
1.8.4

[PATCH RFC v3 06/12] arm: kvm: implement CPU PM notifier

[Lorenzo Pieralisi]

Upon CPU shutdown and consequent warm-reboot, the hypervisor CPU state
must be re-initialized. This patch implements a CPU PM notifier that
upon warm-boot calls a KVM hook to reinitialize properly the hypervisor
state so that the CPU can be safely resumed.

Acked-by: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm/kvm/arm.c | 30 ++++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
index 2a700e0..b18165c 100644
--- a/arch/arm/kvm/arm.c
+++ b/arch/arm/kvm/arm.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/cpu.h>
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/err.h>
 #include <linux/kvm_host.h>
@@ -853,6 +854,33 @@ static struct notifier_block hyp_init_cpu_nb = {
 	.notifier_call = hyp_init_cpu_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static int hyp_init_cpu_pm_notifier(struct notifier_block *self,
+				    unsigned long cmd,
+				    void *v)
+{
+	if (cmd == CPU_PM_EXIT) {
+		cpu_init_hyp_mode(NULL);
+		return NOTIFY_OK;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block hyp_init_cpu_pm_nb = {
+	.notifier_call = hyp_init_cpu_pm_notifier,
+};
+
+static void __init hyp_cpu_pm_init(void)
+{
+	cpu_pm_register_notifier(&hyp_init_cpu_pm_nb);
+}
+#else
+static inline void hyp_cpu_pm_init(void)
+{
+}
+#endif
+
 /**
  * Inits Hyp-mode on all online CPUs
  */
@@ -1013,6 +1041,8 @@ int kvm_arch_init(void *opaque)
 		goto out_err;
 	}
 
+	hyp_cpu_pm_init();
+
 	kvm_coproc_table_init();
 	return 0;
 out_err:
-- 
1.8.4

[PATCH RFC v3 06/12] arm: kvm: implement CPU PM notifier

[Lorenzo Pieralisi]

Upon CPU shutdown and consequent warm-reboot, the hypervisor CPU state
must be re-initialized. This patch implements a CPU PM notifier that
upon warm-boot calls a KVM hook to reinitialize properly the hypervisor
state so that the CPU can be safely resumed.

Acked-by: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm/kvm/arm.c | 30 ++++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
index 2a700e0..b18165c 100644
--- a/arch/arm/kvm/arm.c
+++ b/arch/arm/kvm/arm.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/cpu.h>
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/err.h>
 #include <linux/kvm_host.h>
@@ -853,6 +854,33 @@ static struct notifier_block hyp_init_cpu_nb = {
 	.notifier_call = hyp_init_cpu_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static int hyp_init_cpu_pm_notifier(struct notifier_block *self,
+				    unsigned long cmd,
+				    void *v)
+{
+	if (cmd == CPU_PM_EXIT) {
+		cpu_init_hyp_mode(NULL);
+		return NOTIFY_OK;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block hyp_init_cpu_pm_nb = {
+	.notifier_call = hyp_init_cpu_pm_notifier,
+};
+
+static void __init hyp_cpu_pm_init(void)
+{
+	cpu_pm_register_notifier(&hyp_init_cpu_pm_nb);
+}
+#else
+static inline void hyp_cpu_pm_init(void)
+{
+}
+#endif
+
 /**
  * Inits Hyp-mode on all online CPUs
  */
@@ -1013,6 +1041,8 @@ int kvm_arch_init(void *opaque)
 		goto out_err;
 	}
 
+	hyp_cpu_pm_init();
+
 	kvm_coproc_table_init();
 	return 0;
 out_err:
-- 
1.8.4

[PATCH RFC v3 07/12] arm64: kernel: refactor code to install/uninstall breakpoints

[Lorenzo Pieralisi]

Most of the code executed to install and uninstall breakpoints is
common and can be factored out in a function that through a runtime
operations type provides the requested implementation.

This patch creates a common function that can be used to install/uninstall
breakpoints and defines the set of operations that can be carried out
through it.

Reviewed-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/hw_breakpoint.c | 142 +++++++++++++++++++++++---------------
 1 file changed, 88 insertions(+), 54 deletions(-)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index ff516f6..e894591 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -169,15 +169,63 @@ static enum debug_el debug_exception_level(int privilege)
 	}
 }
 
-/*
- * Install a perf counter breakpoint.
+enum hw_breakpoint_ops {
+	HW_BREAKPOINT_INSTALL,
+	HW_BREAKPOINT_UNINSTALL
+};
+
+/**
+ * hw_breakpoint_slot_setup - Find and setup a perf slot according to
+ *			      operations
+ *
+ * @slots: pointer to array of slots
+ * @max_slots: max number of slots
+ * @bp: perf_event to setup
+ * @ops: operation to be carried out on the slot
+ *
+ * Return:
+ *	slot index on success
+ *	-ENOSPC if no slot is available/matches
+ *	-EINVAL on wrong operations parameter
  */
-int arch_install_hw_breakpoint(struct perf_event *bp)
+static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
+				    struct perf_event *bp,
+				    enum hw_breakpoint_ops ops)
+{
+	int i;
+	struct perf_event **slot;
+
+	for (i = 0; i < max_slots; ++i) {
+		slot = &slots[i];
+		switch (ops) {
+		case HW_BREAKPOINT_INSTALL:
+			if (!*slot) {
+				*slot = bp;
+				return i;
+			}
+			break;
+		case HW_BREAKPOINT_UNINSTALL:
+			if (*slot == bp) {
+				*slot = NULL;
+				return i;
+			}
+			break;
+		default:
+			pr_warn_once("Unhandled hw breakpoint ops %d\n", ops);
+			return -EINVAL;
+		}
+	}
+	return -ENOSPC;
+}
+
+static int hw_breakpoint_control(struct perf_event *bp,
+				 enum hw_breakpoint_ops ops)
 {
 	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-	struct perf_event **slot, **slots;
+	struct perf_event **slots;
 	struct debug_info *debug_info = &current->thread.debug;
 	int i, max_slots, ctrl_reg, val_reg, reg_enable;
+	enum debug_el dbg_el = debug_exception_level(info->ctrl.privilege);
 	u32 ctrl;
 
 	if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
@@ -196,67 +244,53 @@ int arch_install_hw_breakpoint(struct perf_event *bp)
 		reg_enable = !debug_info->wps_disabled;
 	}
 
-	for (i = 0; i < max_slots; ++i) {
-		slot = &slots[i];
+	i = hw_breakpoint_slot_setup(slots, max_slots, bp, ops);
 
-		if (!*slot) {
-			*slot = bp;
-			break;
-		}
-	}
+	if (WARN_ONCE(i < 0, "Can't find any breakpoint slot"))
+		return i;
 
-	if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-		return -ENOSPC;
+	switch (ops) {
+	case HW_BREAKPOINT_INSTALL:
+		/*
+		 * Ensure debug monitors are enabled at the correct exception
+		 * level.
+		 */
+		enable_debug_monitors(dbg_el);
 
-	/* Ensure debug monitors are enabled at the correct exception level.  */
-	enable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+		/* Setup the address register. */
+		write_wb_reg(val_reg, i, info->address);
 
-	/* Setup the address register. */
-	write_wb_reg(val_reg, i, info->address);
+		/* Setup the control register. */
+		ctrl = encode_ctrl_reg(info->ctrl);
+		write_wb_reg(ctrl_reg, i,
+			     reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+		break;
+	case HW_BREAKPOINT_UNINSTALL:
+		/* Reset the control register. */
+		write_wb_reg(ctrl_reg, i, 0);
 
-	/* Setup the control register. */
-	ctrl = encode_ctrl_reg(info->ctrl);
-	write_wb_reg(ctrl_reg, i, reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+		/*
+		 * Release the debug monitors for the correct exception
+		 * level.
+		 */
+		disable_debug_monitors(dbg_el);
+		break;
+	}
 
 	return 0;
 }
 
-void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+/*
+ * Install a perf counter breakpoint.
+ */
+int arch_install_hw_breakpoint(struct perf_event *bp)
 {
-	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-	struct perf_event **slot, **slots;
-	int i, max_slots, base;
-
-	if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
-		/* Breakpoint */
-		base = AARCH64_DBG_REG_BCR;
-		slots = this_cpu_ptr(bp_on_reg);
-		max_slots = core_num_brps;
-	} else {
-		/* Watchpoint */
-		base = AARCH64_DBG_REG_WCR;
-		slots = this_cpu_ptr(wp_on_reg);
-		max_slots = core_num_wrps;
-	}
-
-	/* Remove the breakpoint. */
-	for (i = 0; i < max_slots; ++i) {
-		slot = &slots[i];
-
-		if (*slot == bp) {
-			*slot = NULL;
-			break;
-		}
-	}
-
-	if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-		return;
-
-	/* Reset the control register. */
-	write_wb_reg(base, i, 0);
+	return hw_breakpoint_control(bp, HW_BREAKPOINT_INSTALL);
+}
 
-	/* Release the debug monitors for the correct exception level.  */
-	disable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+{
+	hw_breakpoint_control(bp, HW_BREAKPOINT_UNINSTALL);
 }
 
 static int get_hbp_len(u8 hbp_len)
-- 
1.8.4

[PATCH RFC v3 07/12] arm64: kernel: refactor code to install/uninstall breakpoints

[Lorenzo Pieralisi]

Most of the code executed to install and uninstall breakpoints is
common and can be factored out in a function that through a runtime
operations type provides the requested implementation.

This patch creates a common function that can be used to install/uninstall
breakpoints and defines the set of operations that can be carried out
through it.

Reviewed-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/hw_breakpoint.c | 142 +++++++++++++++++++++++---------------
 1 file changed, 88 insertions(+), 54 deletions(-)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index ff516f6..e894591 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -169,15 +169,63 @@ static enum debug_el debug_exception_level(int privilege)
 	}
 }
 
-/*
- * Install a perf counter breakpoint.
+enum hw_breakpoint_ops {
+	HW_BREAKPOINT_INSTALL,
+	HW_BREAKPOINT_UNINSTALL
+};
+
+/**
+ * hw_breakpoint_slot_setup - Find and setup a perf slot according to
+ *			      operations
+ *
+ * @slots: pointer to array of slots
+ * @max_slots: max number of slots
+ * @bp: perf_event to setup
+ * @ops: operation to be carried out on the slot
+ *
+ * Return:
+ *	slot index on success
+ *	-ENOSPC if no slot is available/matches
+ *	-EINVAL on wrong operations parameter
  */
-int arch_install_hw_breakpoint(struct perf_event *bp)
+static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
+				    struct perf_event *bp,
+				    enum hw_breakpoint_ops ops)
+{
+	int i;
+	struct perf_event **slot;
+
+	for (i = 0; i < max_slots; ++i) {
+		slot = &slots[i];
+		switch (ops) {
+		case HW_BREAKPOINT_INSTALL:
+			if (!*slot) {
+				*slot = bp;
+				return i;
+			}
+			break;
+		case HW_BREAKPOINT_UNINSTALL:
+			if (*slot == bp) {
+				*slot = NULL;
+				return i;
+			}
+			break;
+		default:
+			pr_warn_once("Unhandled hw breakpoint ops %d\n", ops);
+			return -EINVAL;
+		}
+	}
+	return -ENOSPC;
+}
+
+static int hw_breakpoint_control(struct perf_event *bp,
+				 enum hw_breakpoint_ops ops)
 {
 	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-	struct perf_event **slot, **slots;
+	struct perf_event **slots;
 	struct debug_info *debug_info = &current->thread.debug;
 	int i, max_slots, ctrl_reg, val_reg, reg_enable;
+	enum debug_el dbg_el = debug_exception_level(info->ctrl.privilege);
 	u32 ctrl;
 
 	if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
@@ -196,67 +244,53 @@ int arch_install_hw_breakpoint(struct perf_event *bp)
 		reg_enable = !debug_info->wps_disabled;
 	}
 
-	for (i = 0; i < max_slots; ++i) {
-		slot = &slots[i];
+	i = hw_breakpoint_slot_setup(slots, max_slots, bp, ops);
 
-		if (!*slot) {
-			*slot = bp;
-			break;
-		}
-	}
+	if (WARN_ONCE(i < 0, "Can't find any breakpoint slot"))
+		return i;
 
-	if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-		return -ENOSPC;
+	switch (ops) {
+	case HW_BREAKPOINT_INSTALL:
+		/*
+		 * Ensure debug monitors are enabled at the correct exception
+		 * level.
+		 */
+		enable_debug_monitors(dbg_el);
 
-	/* Ensure debug monitors are enabled@the correct exception level.  */
-	enable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+		/* Setup the address register. */
+		write_wb_reg(val_reg, i, info->address);
 
-	/* Setup the address register. */
-	write_wb_reg(val_reg, i, info->address);
+		/* Setup the control register. */
+		ctrl = encode_ctrl_reg(info->ctrl);
+		write_wb_reg(ctrl_reg, i,
+			     reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+		break;
+	case HW_BREAKPOINT_UNINSTALL:
+		/* Reset the control register. */
+		write_wb_reg(ctrl_reg, i, 0);
 
-	/* Setup the control register. */
-	ctrl = encode_ctrl_reg(info->ctrl);
-	write_wb_reg(ctrl_reg, i, reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+		/*
+		 * Release the debug monitors for the correct exception
+		 * level.
+		 */
+		disable_debug_monitors(dbg_el);
+		break;
+	}
 
 	return 0;
 }
 
-void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+/*
+ * Install a perf counter breakpoint.
+ */
+int arch_install_hw_breakpoint(struct perf_event *bp)
 {
-	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-	struct perf_event **slot, **slots;
-	int i, max_slots, base;
-
-	if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
-		/* Breakpoint */
-		base = AARCH64_DBG_REG_BCR;
-		slots = this_cpu_ptr(bp_on_reg);
-		max_slots = core_num_brps;
-	} else {
-		/* Watchpoint */
-		base = AARCH64_DBG_REG_WCR;
-		slots = this_cpu_ptr(wp_on_reg);
-		max_slots = core_num_wrps;
-	}
-
-	/* Remove the breakpoint. */
-	for (i = 0; i < max_slots; ++i) {
-		slot = &slots[i];
-
-		if (*slot == bp) {
-			*slot = NULL;
-			break;
-		}
-	}
-
-	if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-		return;
-
-	/* Reset the control register. */
-	write_wb_reg(base, i, 0);
+	return hw_breakpoint_control(bp, HW_BREAKPOINT_INSTALL);
+}
 
-	/* Release the debug monitors for the correct exception level.  */
-	disable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+{
+	hw_breakpoint_control(bp, HW_BREAKPOINT_UNINSTALL);
 }
 
 static int get_hbp_len(u8 hbp_len)
-- 
1.8.4

[PATCH RFC v3 08/12] arm64: kernel: implement HW breakpoints CPU PM notifier

[Lorenzo Pieralisi]

When a CPU is shutdown either through CPU idle or suspend to RAM, the
content of HW breakpoint registers must be reset or restored to proper
values when CPU resume from low power states. This patch adds debug register
restore operations to the HW breakpoint control function and implements a
CPU PM notifier that allows to restore the content of HW breakpoint registers
to allow proper suspend/resume operations.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/hw_breakpoint.c | 79 ++++++++++++++++++++++++++++++++-------
 1 file changed, 66 insertions(+), 13 deletions(-)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index e894591..bcaaac9 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -20,6 +20,7 @@
 
 #define pr_fmt(fmt) "hw-breakpoint: " fmt
 
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/hw_breakpoint.h>
 #include <linux/perf_event.h>
@@ -171,7 +172,8 @@ static enum debug_el debug_exception_level(int privilege)
 
 enum hw_breakpoint_ops {
 	HW_BREAKPOINT_INSTALL,
-	HW_BREAKPOINT_UNINSTALL
+	HW_BREAKPOINT_UNINSTALL,
+	HW_BREAKPOINT_RESTORE
 };
 
 /**
@@ -210,6 +212,10 @@ static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
 				return i;
 			}
 			break;
+		case HW_BREAKPOINT_RESTORE:
+			if (*slot == bp)
+				return i;
+			break;
 		default:
 			pr_warn_once("Unhandled hw breakpoint ops %d\n", ops);
 			return -EINVAL;
@@ -256,7 +262,8 @@ static int hw_breakpoint_control(struct perf_event *bp,
 		 * level.
 		 */
 		enable_debug_monitors(dbg_el);
-
+		/* Fall through */
+	case HW_BREAKPOINT_RESTORE:
 		/* Setup the address register. */
 		write_wb_reg(val_reg, i, info->address);
 
@@ -840,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
 /*
  * CPU initialisation.
  */
-static void reset_ctrl_regs(void *unused)
+static void hw_breakpoint_reset(void *unused)
 {
 	int i;
-
-	for (i = 0; i < core_num_brps; ++i) {
-		write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
-		write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+	struct perf_event **slots;
+	/*
+	 * When a CPU goes through cold-boot, it does not have any installed
+	 * slot, so it is safe to share the same function for restoring and
+	 * resetting breakpoints; when a CPU is hotplugged in, it goes
+	 * through the slots, which are all empty, hence it just resets control
+	 * and value for debug registers.
+	 * When this function is triggered on warm-boot through a CPU PM
+	 * notifier some slots might be initialized; if so they are
+	 * reprogrammed according to the debug slots content.
+	 */
+	for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
+		if (slots[i]) {
+			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+		} else {
+			write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
+			write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+		}
 	}
 
-	for (i = 0; i < core_num_wrps; ++i) {
-		write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
-		write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+	for (slots = this_cpu_ptr(wp_on_reg), i = 0; i < core_num_wrps; ++i) {
+		if (slots[i]) {
+			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+		} else {
+			write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
+			write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+		}
 	}
 }
 
@@ -861,7 +886,7 @@ static int hw_breakpoint_reset_notify(struct notifier_block *self,
 {
 	int cpu = (long)hcpu;
 	if (action == CPU_ONLINE)
-		smp_call_function_single(cpu, reset_ctrl_regs, NULL, 1);
+		smp_call_function_single(cpu, hw_breakpoint_reset, NULL, 1);
 	return NOTIFY_OK;
 }
 
@@ -869,6 +894,33 @@ static struct notifier_block hw_breakpoint_reset_nb = {
 	.notifier_call = hw_breakpoint_reset_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static int hw_breakpoint_cpu_pm_notify(struct notifier_block *self,
+				       unsigned long action,
+				       void *v)
+{
+	if (action == CPU_PM_EXIT) {
+		hw_breakpoint_reset(NULL);
+		return NOTIFY_OK;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block hw_breakpoint_cpu_pm_nb = {
+	.notifier_call = hw_breakpoint_cpu_pm_notify,
+};
+
+static void __init hw_breakpoint_pm_init(void)
+{
+	cpu_pm_register_notifier(&hw_breakpoint_cpu_pm_nb);
+}
+#else
+static inline void hw_breakpoint_pm_init(void)
+{
+}
+#endif
+
 /*
  * One-time initialisation.
  */
@@ -884,8 +936,8 @@ static int __init arch_hw_breakpoint_init(void)
 	 * Reset the breakpoint resources. We assume that a halting
 	 * debugger will leave the world in a nice state for us.
 	 */
-	smp_call_function(reset_ctrl_regs, NULL, 1);
-	reset_ctrl_regs(NULL);
+	smp_call_function(hw_breakpoint_reset, NULL, 1);
+	hw_breakpoint_reset(NULL);
 
 	/* Register debug fault handlers. */
 	hook_debug_fault_code(DBG_ESR_EVT_HWBP, breakpoint_handler, SIGTRAP,
@@ -895,6 +947,7 @@ static int __init arch_hw_breakpoint_init(void)
 
 	/* Register hotplug notifier. */
 	register_cpu_notifier(&hw_breakpoint_reset_nb);
+	hw_breakpoint_pm_init();
 
 	return 0;
 }
-- 
1.8.4

[PATCH RFC v3 08/12] arm64: kernel: implement HW breakpoints CPU PM notifier

[Lorenzo Pieralisi]

When a CPU is shutdown either through CPU idle or suspend to RAM, the
content of HW breakpoint registers must be reset or restored to proper
values when CPU resume from low power states. This patch adds debug register
restore operations to the HW breakpoint control function and implements a
CPU PM notifier that allows to restore the content of HW breakpoint registers
to allow proper suspend/resume operations.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/hw_breakpoint.c | 79 ++++++++++++++++++++++++++++++++-------
 1 file changed, 66 insertions(+), 13 deletions(-)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index e894591..bcaaac9 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -20,6 +20,7 @@
 
 #define pr_fmt(fmt) "hw-breakpoint: " fmt
 
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/hw_breakpoint.h>
 #include <linux/perf_event.h>
@@ -171,7 +172,8 @@ static enum debug_el debug_exception_level(int privilege)
 
 enum hw_breakpoint_ops {
 	HW_BREAKPOINT_INSTALL,
-	HW_BREAKPOINT_UNINSTALL
+	HW_BREAKPOINT_UNINSTALL,
+	HW_BREAKPOINT_RESTORE
 };
 
 /**
@@ -210,6 +212,10 @@ static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
 				return i;
 			}
 			break;
+		case HW_BREAKPOINT_RESTORE:
+			if (*slot == bp)
+				return i;
+			break;
 		default:
 			pr_warn_once("Unhandled hw breakpoint ops %d\n", ops);
 			return -EINVAL;
@@ -256,7 +262,8 @@ static int hw_breakpoint_control(struct perf_event *bp,
 		 * level.
 		 */
 		enable_debug_monitors(dbg_el);
-
+		/* Fall through */
+	case HW_BREAKPOINT_RESTORE:
 		/* Setup the address register. */
 		write_wb_reg(val_reg, i, info->address);
 
@@ -840,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
 /*
  * CPU initialisation.
  */
-static void reset_ctrl_regs(void *unused)
+static void hw_breakpoint_reset(void *unused)
 {
 	int i;
-
-	for (i = 0; i < core_num_brps; ++i) {
-		write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
-		write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+	struct perf_event **slots;
+	/*
+	 * When a CPU goes through cold-boot, it does not have any installed
+	 * slot, so it is safe to share the same function for restoring and
+	 * resetting breakpoints; when a CPU is hotplugged in, it goes
+	 * through the slots, which are all empty, hence it just resets control
+	 * and value for debug registers.
+	 * When this function is triggered on warm-boot through a CPU PM
+	 * notifier some slots might be initialized; if so they are
+	 * reprogrammed according to the debug slots content.
+	 */
+	for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
+		if (slots[i]) {
+			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+		} else {
+			write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
+			write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+		}
 	}
 
-	for (i = 0; i < core_num_wrps; ++i) {
-		write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
-		write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+	for (slots = this_cpu_ptr(wp_on_reg), i = 0; i < core_num_wrps; ++i) {
+		if (slots[i]) {
+			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+		} else {
+			write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
+			write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+		}
 	}
 }
 
@@ -861,7 +886,7 @@ static int hw_breakpoint_reset_notify(struct notifier_block *self,
 {
 	int cpu = (long)hcpu;
 	if (action == CPU_ONLINE)
-		smp_call_function_single(cpu, reset_ctrl_regs, NULL, 1);
+		smp_call_function_single(cpu, hw_breakpoint_reset, NULL, 1);
 	return NOTIFY_OK;
 }
 
@@ -869,6 +894,33 @@ static struct notifier_block hw_breakpoint_reset_nb = {
 	.notifier_call = hw_breakpoint_reset_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static int hw_breakpoint_cpu_pm_notify(struct notifier_block *self,
+				       unsigned long action,
+				       void *v)
+{
+	if (action == CPU_PM_EXIT) {
+		hw_breakpoint_reset(NULL);
+		return NOTIFY_OK;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block hw_breakpoint_cpu_pm_nb = {
+	.notifier_call = hw_breakpoint_cpu_pm_notify,
+};
+
+static void __init hw_breakpoint_pm_init(void)
+{
+	cpu_pm_register_notifier(&hw_breakpoint_cpu_pm_nb);
+}
+#else
+static inline void hw_breakpoint_pm_init(void)
+{
+}
+#endif
+
 /*
  * One-time initialisation.
  */
@@ -884,8 +936,8 @@ static int __init arch_hw_breakpoint_init(void)
 	 * Reset the breakpoint resources. We assume that a halting
 	 * debugger will leave the world in a nice state for us.
 	 */
-	smp_call_function(reset_ctrl_regs, NULL, 1);
-	reset_ctrl_regs(NULL);
+	smp_call_function(hw_breakpoint_reset, NULL, 1);
+	hw_breakpoint_reset(NULL);
 
 	/* Register debug fault handlers. */
 	hook_debug_fault_code(DBG_ESR_EVT_HWBP, breakpoint_handler, SIGTRAP,
@@ -895,6 +947,7 @@ static int __init arch_hw_breakpoint_init(void)
 
 	/* Register hotplug notifier. */
 	register_cpu_notifier(&hw_breakpoint_reset_nb);
+	hw_breakpoint_pm_init();
 
 	return 0;
 }
-- 
1.8.4

[PATCH RFC v3 09/12] arm64: enable generic clockevent broadcast

[Lorenzo Pieralisi]

On platforms with power management capabilities, timers that are shut
down when a CPU enters deep C-states must be emulated using an always-on
timer and a timer IPI to relay the timer IRQ to target CPUs on an SMP
system.

This patch enables the generic clockevents broadcast infrastructure for
arm64, by providing the required Kconfig entries and adding the timer
IPI infrastructure.

Acked-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/Kconfig               |  2 ++
 arch/arm64/include/asm/hardirq.h |  2 +-
 arch/arm64/kernel/smp.c          | 17 +++++++++++++++++
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 88c8b6c1..1d222b1 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -2,6 +2,7 @@ config ARM64
 	def_bool y
 	select ARCH_HAS_ATOMIC64_DEC_IF_POSITIVE
 	select ARCH_USE_CMPXCHG_LOCKREF
+	select ARCH_HAS_TICK_BROADCAST if GENERIC_CLOCKEVENTS_BROADCAST
 	select ARCH_WANT_OPTIONAL_GPIOLIB
 	select ARCH_WANT_COMPAT_IPC_PARSE_VERSION
 	select ARCH_WANT_FRAME_POINTERS
@@ -12,6 +13,7 @@ config ARM64
 	select CLONE_BACKWARDS
 	select COMMON_CLK
 	select GENERIC_CLOCKEVENTS
+	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
 	select GENERIC_IOMAP
 	select GENERIC_IRQ_PROBE
 	select GENERIC_IRQ_SHOW
diff --git a/arch/arm64/include/asm/hardirq.h b/arch/arm64/include/asm/hardirq.h
index 990c051..ae4801d 100644
--- a/arch/arm64/include/asm/hardirq.h
+++ b/arch/arm64/include/asm/hardirq.h
@@ -20,7 +20,7 @@
 #include <linux/threads.h>
 #include <asm/irq.h>
 
-#define NR_IPI	4
+#define NR_IPI	5
 
 typedef struct {
 	unsigned int __softirq_pending;
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index a5aeefa..56b68d7 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -61,6 +61,7 @@ enum ipi_msg_type {
 	IPI_CALL_FUNC,
 	IPI_CALL_FUNC_SINGLE,
 	IPI_CPU_STOP,
+	IPI_TIMER,
 };
 
 /*
@@ -446,6 +447,7 @@ static const char *ipi_types[NR_IPI] = {
 	S(IPI_CALL_FUNC, "Function call interrupts"),
 	S(IPI_CALL_FUNC_SINGLE, "Single function call interrupts"),
 	S(IPI_CPU_STOP, "CPU stop interrupts"),
+	S(IPI_TIMER, "Timer broadcast interrupts"),
 };
 
 void show_ipi_list(struct seq_file *p, int prec)
@@ -531,6 +533,14 @@ void handle_IPI(int ipinr, struct pt_regs *regs)
 		irq_exit();
 		break;
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+	case IPI_TIMER:
+		irq_enter();
+		tick_receive_broadcast();
+		irq_exit();
+		break;
+#endif
+
 	default:
 		pr_crit("CPU%u: Unknown IPI message 0x%x\n", cpu, ipinr);
 		break;
@@ -543,6 +553,13 @@ void smp_send_reschedule(int cpu)
 	smp_cross_call(cpumask_of(cpu), IPI_RESCHEDULE);
 }
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+void tick_broadcast(const struct cpumask *mask)
+{
+	smp_cross_call(mask, IPI_TIMER);
+}
+#endif
+
 void smp_send_stop(void)
 {
 	unsigned long timeout;
-- 
1.8.4

[PATCH RFC v3 09/12] arm64: enable generic clockevent broadcast

[Lorenzo Pieralisi]

On platforms with power management capabilities, timers that are shut
down when a CPU enters deep C-states must be emulated using an always-on
timer and a timer IPI to relay the timer IRQ to target CPUs on an SMP
system.

This patch enables the generic clockevents broadcast infrastructure for
arm64, by providing the required Kconfig entries and adding the timer
IPI infrastructure.

Acked-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/Kconfig               |  2 ++
 arch/arm64/include/asm/hardirq.h |  2 +-
 arch/arm64/kernel/smp.c          | 17 +++++++++++++++++
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 88c8b6c1..1d222b1 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -2,6 +2,7 @@ config ARM64
 	def_bool y
 	select ARCH_HAS_ATOMIC64_DEC_IF_POSITIVE
 	select ARCH_USE_CMPXCHG_LOCKREF
+	select ARCH_HAS_TICK_BROADCAST if GENERIC_CLOCKEVENTS_BROADCAST
 	select ARCH_WANT_OPTIONAL_GPIOLIB
 	select ARCH_WANT_COMPAT_IPC_PARSE_VERSION
 	select ARCH_WANT_FRAME_POINTERS
@@ -12,6 +13,7 @@ config ARM64
 	select CLONE_BACKWARDS
 	select COMMON_CLK
 	select GENERIC_CLOCKEVENTS
+	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
 	select GENERIC_IOMAP
 	select GENERIC_IRQ_PROBE
 	select GENERIC_IRQ_SHOW
diff --git a/arch/arm64/include/asm/hardirq.h b/arch/arm64/include/asm/hardirq.h
index 990c051..ae4801d 100644
--- a/arch/arm64/include/asm/hardirq.h
+++ b/arch/arm64/include/asm/hardirq.h
@@ -20,7 +20,7 @@
 #include <linux/threads.h>
 #include <asm/irq.h>
 
-#define NR_IPI	4
+#define NR_IPI	5
 
 typedef struct {
 	unsigned int __softirq_pending;
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index a5aeefa..56b68d7 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -61,6 +61,7 @@ enum ipi_msg_type {
 	IPI_CALL_FUNC,
 	IPI_CALL_FUNC_SINGLE,
 	IPI_CPU_STOP,
+	IPI_TIMER,
 };
 
 /*
@@ -446,6 +447,7 @@ static const char *ipi_types[NR_IPI] = {
 	S(IPI_CALL_FUNC, "Function call interrupts"),
 	S(IPI_CALL_FUNC_SINGLE, "Single function call interrupts"),
 	S(IPI_CPU_STOP, "CPU stop interrupts"),
+	S(IPI_TIMER, "Timer broadcast interrupts"),
 };
 
 void show_ipi_list(struct seq_file *p, int prec)
@@ -531,6 +533,14 @@ void handle_IPI(int ipinr, struct pt_regs *regs)
 		irq_exit();
 		break;
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+	case IPI_TIMER:
+		irq_enter();
+		tick_receive_broadcast();
+		irq_exit();
+		break;
+#endif
+
 	default:
 		pr_crit("CPU%u: Unknown IPI message 0x%x\n", cpu, ipinr);
 		break;
@@ -543,6 +553,13 @@ void smp_send_reschedule(int cpu)
 	smp_cross_call(cpumask_of(cpu), IPI_RESCHEDULE);
 }
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+void tick_broadcast(const struct cpumask *mask)
+{
+	smp_cross_call(mask, IPI_TIMER);
+}
+#endif
+
 void smp_send_stop(void)
 {
 	unsigned long timeout;
-- 
1.8.4

[PATCH RFC v3 10/12] arm64: kernel: add CPU idle call

[Lorenzo Pieralisi]

When CPU idle is enabled, the architectural idle call should go through
the idle subsystem to allow CPUs to enter idle states defined
by the platform CPU idle back-end operations.

This patch, mirroring other archs behaviour, adds the CPU idle call to the
architectural arch_cpu_idle implementation for arm64.

Acked-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/process.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/arch/arm64/kernel/process.c b/arch/arm64/kernel/process.c
index de17c89..50491ec 100644
--- a/arch/arm64/kernel/process.c
+++ b/arch/arm64/kernel/process.c
@@ -33,6 +33,7 @@
 #include <linux/kallsyms.h>
 #include <linux/init.h>
 #include <linux/cpu.h>
+#include <linux/cpuidle.h>
 #include <linux/elfcore.h>
 #include <linux/pm.h>
 #include <linux/tick.h>
@@ -98,8 +99,10 @@ void arch_cpu_idle(void)
 	 * This should do all the clock switching and wait for interrupt
 	 * tricks
 	 */
-	cpu_do_idle();
-	local_irq_enable();
+	if (cpuidle_idle_call()) {
+		cpu_do_idle();
+		local_irq_enable();
+	}
 }
 
 #ifdef CONFIG_HOTPLUG_CPU
-- 
1.8.4

[PATCH RFC v3 10/12] arm64: kernel: add CPU idle call

[Lorenzo Pieralisi]

When CPU idle is enabled, the architectural idle call should go through
the idle subsystem to allow CPUs to enter idle states defined
by the platform CPU idle back-end operations.

This patch, mirroring other archs behaviour, adds the CPU idle call to the
architectural arch_cpu_idle implementation for arm64.

Acked-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/kernel/process.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/arch/arm64/kernel/process.c b/arch/arm64/kernel/process.c
index de17c89..50491ec 100644
--- a/arch/arm64/kernel/process.c
+++ b/arch/arm64/kernel/process.c
@@ -33,6 +33,7 @@
 #include <linux/kallsyms.h>
 #include <linux/init.h>
 #include <linux/cpu.h>
+#include <linux/cpuidle.h>
 #include <linux/elfcore.h>
 #include <linux/pm.h>
 #include <linux/tick.h>
@@ -98,8 +99,10 @@ void arch_cpu_idle(void)
 	 * This should do all the clock switching and wait for interrupt
 	 * tricks
 	 */
-	cpu_do_idle();
-	local_irq_enable();
+	if (cpuidle_idle_call()) {
+		cpu_do_idle();
+		local_irq_enable();
+	}
 }
 
 #ifdef CONFIG_HOTPLUG_CPU
-- 
1.8.4

[PATCH RFC v3 11/12] arm64: kernel: add PM build infrastructure

[Lorenzo Pieralisi]

This patch adds the required makefile and kconfig entries to enable PM
for arm64 systems.

The kernel relies on the cpu_{suspend}/{resume} infrastructure to
properly save the context for a CPU and put it to sleep, hence this
patch adds the config option required to enable cpu_{suspend}/{resume}
API.

In order to rely on the CPU PM implementation for saving and restoring
of CPU subsystems like GIC and PMU, the arch Kconfig must be also
augmented to select the CONFIG_CPU_PM option when SUSPEND or CPU_IDLE
kernel implementations are selected.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/Kconfig         | 13 +++++++++++++
 arch/arm64/kernel/Makefile |  1 +
 2 files changed, 14 insertions(+)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 1d222b1..0d78e92 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -12,6 +12,7 @@ config ARM64
 	select BUILDTIME_EXTABLE_SORT
 	select CLONE_BACKWARDS
 	select COMMON_CLK
+	select CPU_PM if (SUSPEND || CPU_IDLE)
 	select GENERIC_CLOCKEVENTS
 	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
 	select GENERIC_IOMAP
@@ -278,6 +279,18 @@ config SYSVIPC_COMPAT
 
 endmenu
 
+menu "Power management options"
+
+source "kernel/power/Kconfig"
+
+config ARCH_SUSPEND_POSSIBLE
+	def_bool y
+
+config ARM64_CPU_SUSPEND
+	def_bool PM_SLEEP
+
+endmenu
+
 source "net/Kconfig"
 
 source "drivers/Kconfig"
diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 5ba2fd4..1cd339d 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -18,6 +18,7 @@ arm64-obj-$(CONFIG_SMP)			+= smp.o smp_spin_table.o
 arm64-obj-$(CONFIG_HW_PERF_EVENTS)	+= perf_event.o
 arm64-obj-$(CONFIG_HAVE_HW_BREAKPOINT)+= hw_breakpoint.o
 arm64-obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
+arm64-obj-$(CONFIG_ARM64_CPU_SUSPEND)	+= sleep.o suspend.o
 
 obj-y					+= $(arm64-obj-y) vdso/
 obj-m					+= $(arm64-obj-m)
-- 
1.8.4

[PATCH RFC v3 11/12] arm64: kernel: add PM build infrastructure

[Lorenzo Pieralisi]

This patch adds the required makefile and kconfig entries to enable PM
for arm64 systems.

The kernel relies on the cpu_{suspend}/{resume} infrastructure to
properly save the context for a CPU and put it to sleep, hence this
patch adds the config option required to enable cpu_{suspend}/{resume}
API.

In order to rely on the CPU PM implementation for saving and restoring
of CPU subsystems like GIC and PMU, the arch Kconfig must be also
augmented to select the CONFIG_CPU_PM option when SUSPEND or CPU_IDLE
kernel implementations are selected.

Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/Kconfig         | 13 +++++++++++++
 arch/arm64/kernel/Makefile |  1 +
 2 files changed, 14 insertions(+)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 1d222b1..0d78e92 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -12,6 +12,7 @@ config ARM64
 	select BUILDTIME_EXTABLE_SORT
 	select CLONE_BACKWARDS
 	select COMMON_CLK
+	select CPU_PM if (SUSPEND || CPU_IDLE)
 	select GENERIC_CLOCKEVENTS
 	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
 	select GENERIC_IOMAP
@@ -278,6 +279,18 @@ config SYSVIPC_COMPAT
 
 endmenu
 
+menu "Power management options"
+
+source "kernel/power/Kconfig"
+
+config ARCH_SUSPEND_POSSIBLE
+	def_bool y
+
+config ARM64_CPU_SUSPEND
+	def_bool PM_SLEEP
+
+endmenu
+
 source "net/Kconfig"
 
 source "drivers/Kconfig"
diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 5ba2fd4..1cd339d 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -18,6 +18,7 @@ arm64-obj-$(CONFIG_SMP)			+= smp.o smp_spin_table.o
 arm64-obj-$(CONFIG_HW_PERF_EVENTS)	+= perf_event.o
 arm64-obj-$(CONFIG_HAVE_HW_BREAKPOINT)+= hw_breakpoint.o
 arm64-obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
+arm64-obj-$(CONFIG_ARM64_CPU_SUSPEND)	+= sleep.o suspend.o
 
 obj-y					+= $(arm64-obj-y) vdso/
 obj-m					+= $(arm64-obj-m)
-- 
1.8.4

[PATCH RFC v3 12/12] arm64: add CPU power management menu/entries

[Lorenzo Pieralisi]

This patch provides a menu for CPU power management options in the
arm64 Kconfig and adds an entry to enable the generic CPU idle configuration.

Acked-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/Kconfig | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 0d78e92..3e2c392 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -291,6 +291,12 @@ config ARM64_CPU_SUSPEND
 
 endmenu
 
+menu "CPU Power Management"
+
+source "drivers/cpuidle/Kconfig"
+
+endmenu
+
 source "net/Kconfig"
 
 source "drivers/Kconfig"
-- 
1.8.4

[PATCH RFC v3 12/12] arm64: add CPU power management menu/entries

[Lorenzo Pieralisi]

This patch provides a menu for CPU power management options in the
arm64 Kconfig and adds an entry to enable the generic CPU idle configuration.

Acked-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
---
 arch/arm64/Kconfig | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 0d78e92..3e2c392 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -291,6 +291,12 @@ config ARM64_CPU_SUSPEND
 
 endmenu
 
+menu "CPU Power Management"
+
+source "drivers/cpuidle/Kconfig"
+
+endmenu
+
 source "net/Kconfig"
 
 source "drivers/Kconfig"
-- 
1.8.4

Re: [PATCH RFC v3 06/12] arm: kvm: implement CPU PM notifier

[Christoffer Dall]

On Thu, Nov 21, 2013 at 11:24:13AM +0000, Lorenzo Pieralisi wrote:
> Upon CPU shutdown and consequent warm-reboot, the hypervisor CPU state
> must be re-initialized. This patch implements a CPU PM notifier that
> upon warm-boot calls a KVM hook to reinitialize properly the hypervisor
> state so that the CPU can be safely resumed.
> 
> Acked-by: Marc Zyngier <marc.zyngier@arm.com>
> Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
> ---
>  arch/arm/kvm/arm.c | 30 ++++++++++++++++++++++++++++++
>  1 file changed, 30 insertions(+)
> 
> diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
> index 2a700e0..b18165c 100644
> --- a/arch/arm/kvm/arm.c
> +++ b/arch/arm/kvm/arm.c
> @@ -17,6 +17,7 @@
>   */
>  
>  #include <linux/cpu.h>
> +#include <linux/cpu_pm.h>
>  #include <linux/errno.h>
>  #include <linux/err.h>
>  #include <linux/kvm_host.h>
> @@ -853,6 +854,33 @@ static struct notifier_block hyp_init_cpu_nb = {
>  	.notifier_call = hyp_init_cpu_notify,
>  };
>  
> +#ifdef CONFIG_CPU_PM
> +static int hyp_init_cpu_pm_notifier(struct notifier_block *self,
> +				    unsigned long cmd,
> +				    void *v)
> +{
> +	if (cmd == CPU_PM_EXIT) {
> +		cpu_init_hyp_mode(NULL);
> +		return NOTIFY_OK;
> +	}
> +
> +	return NOTIFY_DONE;
> +}
> +
> +static struct notifier_block hyp_init_cpu_pm_nb = {
> +	.notifier_call = hyp_init_cpu_pm_notifier,
> +};
> +
> +static void __init hyp_cpu_pm_init(void)
> +{
> +	cpu_pm_register_notifier(&hyp_init_cpu_pm_nb);
> +}
> +#else
> +static inline void hyp_cpu_pm_init(void)
> +{
> +}
> +#endif
> +
>  /**
>   * Inits Hyp-mode on all online CPUs
>   */
> @@ -1013,6 +1041,8 @@ int kvm_arch_init(void *opaque)
>  		goto out_err;
>  	}
>  
> +	hyp_cpu_pm_init();
> +
>  	kvm_coproc_table_init();
>  	return 0;
>  out_err:
> -- 
> 1.8.4
> 
> 
Acked-by: Christoffer Dall <christoffer.dall@linaro.org>

[PATCH RFC v3 06/12] arm: kvm: implement CPU PM notifier

[Christoffer Dall]

On Thu, Nov 21, 2013 at 11:24:13AM +0000, Lorenzo Pieralisi wrote:
> Upon CPU shutdown and consequent warm-reboot, the hypervisor CPU state
> must be re-initialized. This patch implements a CPU PM notifier that
> upon warm-boot calls a KVM hook to reinitialize properly the hypervisor
> state so that the CPU can be safely resumed.
> 
> Acked-by: Marc Zyngier <marc.zyngier@arm.com>
> Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
> ---
>  arch/arm/kvm/arm.c | 30 ++++++++++++++++++++++++++++++
>  1 file changed, 30 insertions(+)
> 
> diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
> index 2a700e0..b18165c 100644
> --- a/arch/arm/kvm/arm.c
> +++ b/arch/arm/kvm/arm.c
> @@ -17,6 +17,7 @@
>   */
>  
>  #include <linux/cpu.h>
> +#include <linux/cpu_pm.h>
>  #include <linux/errno.h>
>  #include <linux/err.h>
>  #include <linux/kvm_host.h>
> @@ -853,6 +854,33 @@ static struct notifier_block hyp_init_cpu_nb = {
>  	.notifier_call = hyp_init_cpu_notify,
>  };
>  
> +#ifdef CONFIG_CPU_PM
> +static int hyp_init_cpu_pm_notifier(struct notifier_block *self,
> +				    unsigned long cmd,
> +				    void *v)
> +{
> +	if (cmd == CPU_PM_EXIT) {
> +		cpu_init_hyp_mode(NULL);
> +		return NOTIFY_OK;
> +	}
> +
> +	return NOTIFY_DONE;
> +}
> +
> +static struct notifier_block hyp_init_cpu_pm_nb = {
> +	.notifier_call = hyp_init_cpu_pm_notifier,
> +};
> +
> +static void __init hyp_cpu_pm_init(void)
> +{
> +	cpu_pm_register_notifier(&hyp_init_cpu_pm_nb);
> +}
> +#else
> +static inline void hyp_cpu_pm_init(void)
> +{
> +}
> +#endif
> +
>  /**
>   * Inits Hyp-mode on all online CPUs
>   */
> @@ -1013,6 +1041,8 @@ int kvm_arch_init(void *opaque)
>  		goto out_err;
>  	}
>  
> +	hyp_cpu_pm_init();
> +
>  	kvm_coproc_table_init();
>  	return 0;
>  out_err:
> -- 
> 1.8.4
> 
> 
Acked-by: Christoffer Dall <christoffer.dall@linaro.org>

Re: [PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Leo Yan]

On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:

> +/*
> + * x0 must contain the sctlr value retrieved from restored context
> + */
> +ENTRY(cpu_resume_mmu)
> +	ldr	x3, =cpu_resume_after_mmu
> +	msr	sctlr_el1, x0		// restore sctlr_el1
> +	isb
> +	br	x3			// global jump to virtual address
> +ENDPROC(cpu_resume_mmu)
> +cpu_resume_after_mmu:
> +	mov	x0, #0			// return zero on success
> +	ldp	x19, x20, [sp, #16]
> +	ldp	x21, x22, [sp, #32]
> +	ldp	x23, x24, [sp, #48]
> +	ldp	x25, x26, [sp, #64]
> +	ldp	x27, x28, [sp, #80]
> +	ldp	x29, lr, [sp], #96
> +	ret
> +ENDPROC(cpu_resume_after_mmu)
> +
> +	.data
> +ENTRY(cpu_resume)
> +	bl	el2_setup		// if in EL2 drop to EL1 cleanly

Compare to v2's patch set, here remove the calculation fro the offset 
b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28 
is zero and finally introduce the EL2's sync exception. Below are pasted 
v2's code for reference.

do u want use firmware to set the x28 for the offset value? :-) IMHO, 
v2's implementation is more reasonable and it's better keep the code.

ENTRY(cpu_resume)
           adr     x4, sleep_save_sp
           ldr     x5, =sleep_save_sp
           sub     x28, x4, x5             // x28 = PHYS_OFFSET - 
PAGE_OFFSET
           /*
            * make sure el2 is sane, el2_setup expects:
            * x28 = PHYS_OFFSET - PAGE_OFFSET
            */
           bl      el2_setup               // if in EL2 drop to EL1 cleanly


> +#ifdef CONFIG_SMP
> +	mrs	x1, mpidr_el1
> +	adr	x4, mpidr_hash_ptr
> +	ldr	x5, [x4]
> +	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
> +        /* retrieve mpidr_hash members to compute the hash */
> +	ldr	x2, [x8, #MPIDR_HASH_MASK]
> +	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
> +	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
> +	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
> +        /* x7 contains hash index, let's use it to grab context pointer */
> +#else
> +	mov	x7, xzr
> +#endif
> +	adr	x0, sleep_save_sp
> +	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
> +	ldr	x0, [x0, x7, lsl #3]
> +	/* load sp from context */
> +	ldr	x2, [x0, #CPU_CTX_SP]
> +	adr	x1, sleep_idmap_phys
> +	/* load physical address of identity map page table in x1 */
> +	ldr	x1, [x1]
> +	mov	sp, x2
> +	/*
> +	 * cpu_do_resume expects x0 to contain context physical address
> +	 * pointer and x1 to contain physical address of 1:1 page tables
> +	 */
> +	bl	cpu_do_resume		// PC relative jump, MMU off
> +	b	cpu_resume_mmu		// Resume MMU, never returns
> +ENDPROC(cpu_resume)
> +

[PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Leo Yan]

On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:

> +/*
> + * x0 must contain the sctlr value retrieved from restored context
> + */
> +ENTRY(cpu_resume_mmu)
> +	ldr	x3, =cpu_resume_after_mmu
> +	msr	sctlr_el1, x0		// restore sctlr_el1
> +	isb
> +	br	x3			// global jump to virtual address
> +ENDPROC(cpu_resume_mmu)
> +cpu_resume_after_mmu:
> +	mov	x0, #0			// return zero on success
> +	ldp	x19, x20, [sp, #16]
> +	ldp	x21, x22, [sp, #32]
> +	ldp	x23, x24, [sp, #48]
> +	ldp	x25, x26, [sp, #64]
> +	ldp	x27, x28, [sp, #80]
> +	ldp	x29, lr, [sp], #96
> +	ret
> +ENDPROC(cpu_resume_after_mmu)
> +
> +	.data
> +ENTRY(cpu_resume)
> +	bl	el2_setup		// if in EL2 drop to EL1 cleanly

Compare to v2's patch set, here remove the calculation fro the offset 
b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28 
is zero and finally introduce the EL2's sync exception. Below are pasted 
v2's code for reference.

do u want use firmware to set the x28 for the offset value? :-) IMHO, 
v2's implementation is more reasonable and it's better keep the code.

ENTRY(cpu_resume)
           adr     x4, sleep_save_sp
           ldr     x5, =sleep_save_sp
           sub     x28, x4, x5             // x28 = PHYS_OFFSET - 
PAGE_OFFSET
           /*
            * make sure el2 is sane, el2_setup expects:
            * x28 = PHYS_OFFSET - PAGE_OFFSET
            */
           bl      el2_setup               // if in EL2 drop to EL1 cleanly


> +#ifdef CONFIG_SMP
> +	mrs	x1, mpidr_el1
> +	adr	x4, mpidr_hash_ptr
> +	ldr	x5, [x4]
> +	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
> +        /* retrieve mpidr_hash members to compute the hash */
> +	ldr	x2, [x8, #MPIDR_HASH_MASK]
> +	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
> +	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
> +	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
> +        /* x7 contains hash index, let's use it to grab context pointer */
> +#else
> +	mov	x7, xzr
> +#endif
> +	adr	x0, sleep_save_sp
> +	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
> +	ldr	x0, [x0, x7, lsl #3]
> +	/* load sp from context */
> +	ldr	x2, [x0, #CPU_CTX_SP]
> +	adr	x1, sleep_idmap_phys
> +	/* load physical address of identity map page table in x1 */
> +	ldr	x1, [x1]
> +	mov	sp, x2
> +	/*
> +	 * cpu_do_resume expects x0 to contain context physical address
> +	 * pointer and x1 to contain physical address of 1:1 page tables
> +	 */
> +	bl	cpu_do_resume		// PC relative jump, MMU off
> +	b	cpu_resume_mmu		// Resume MMU, never returns
> +ENDPROC(cpu_resume)
> +

Re: [PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Catalin Marinas]

On Fri, Dec 20, 2013 at 11:30:16AM +0000, Leo Yan wrote:
> On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:
> > +/*
> > + * x0 must contain the sctlr value retrieved from restored context
> > + */
> > +ENTRY(cpu_resume_mmu)
> > +	ldr	x3, =cpu_resume_after_mmu
> > +	msr	sctlr_el1, x0		// restore sctlr_el1
> > +	isb
> > +	br	x3			// global jump to virtual address
> > +ENDPROC(cpu_resume_mmu)
> > +cpu_resume_after_mmu:
> > +	mov	x0, #0			// return zero on success
> > +	ldp	x19, x20, [sp, #16]
> > +	ldp	x21, x22, [sp, #32]
> > +	ldp	x23, x24, [sp, #48]
> > +	ldp	x25, x26, [sp, #64]
> > +	ldp	x27, x28, [sp, #80]
> > +	ldp	x29, lr, [sp], #96
> > +	ret
> > +ENDPROC(cpu_resume_after_mmu)
> > +
> > +	.data
> > +ENTRY(cpu_resume)
> > +	bl	el2_setup		// if in EL2 drop to EL1 cleanly
> 
> Compare to v2's patch set, here remove the calculation fro the offset 
> b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28 
> is zero and finally introduce the EL2's sync exception. Below are pasted 
> v2's code for reference.
> 
> do u want use firmware to set the x28 for the offset value? :-) IMHO, 
> v2's implementation is more reasonable and it's better keep the code.
> 
> ENTRY(cpu_resume)
>            adr     x4, sleep_save_sp
>            ldr     x5, =sleep_save_sp
>            sub     x28, x4, x5             // x28 = PHYS_OFFSET - 
> PAGE_OFFSET
>            /*
>             * make sure el2 is sane, el2_setup expects:
>             * x28 = PHYS_OFFSET - PAGE_OFFSET
>             */
>            bl      el2_setup               // if in EL2 drop to EL1 cleanly

With commit 85cc00eaa81d (arm64: kernel: add code to set cpu boot mode
to secondary_entry shim) we no longer compute the phys-page offset
before el2_setup.

Where does the fault happen?

-- 
Catalin

[PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Catalin Marinas]

On Fri, Dec 20, 2013 at 11:30:16AM +0000, Leo Yan wrote:
> On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:
> > +/*
> > + * x0 must contain the sctlr value retrieved from restored context
> > + */
> > +ENTRY(cpu_resume_mmu)
> > +	ldr	x3, =cpu_resume_after_mmu
> > +	msr	sctlr_el1, x0		// restore sctlr_el1
> > +	isb
> > +	br	x3			// global jump to virtual address
> > +ENDPROC(cpu_resume_mmu)
> > +cpu_resume_after_mmu:
> > +	mov	x0, #0			// return zero on success
> > +	ldp	x19, x20, [sp, #16]
> > +	ldp	x21, x22, [sp, #32]
> > +	ldp	x23, x24, [sp, #48]
> > +	ldp	x25, x26, [sp, #64]
> > +	ldp	x27, x28, [sp, #80]
> > +	ldp	x29, lr, [sp], #96
> > +	ret
> > +ENDPROC(cpu_resume_after_mmu)
> > +
> > +	.data
> > +ENTRY(cpu_resume)
> > +	bl	el2_setup		// if in EL2 drop to EL1 cleanly
> 
> Compare to v2's patch set, here remove the calculation fro the offset 
> b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28 
> is zero and finally introduce the EL2's sync exception. Below are pasted 
> v2's code for reference.
> 
> do u want use firmware to set the x28 for the offset value? :-) IMHO, 
> v2's implementation is more reasonable and it's better keep the code.
> 
> ENTRY(cpu_resume)
>            adr     x4, sleep_save_sp
>            ldr     x5, =sleep_save_sp
>            sub     x28, x4, x5             // x28 = PHYS_OFFSET - 
> PAGE_OFFSET
>            /*
>             * make sure el2 is sane, el2_setup expects:
>             * x28 = PHYS_OFFSET - PAGE_OFFSET
>             */
>            bl      el2_setup               // if in EL2 drop to EL1 cleanly

With commit 85cc00eaa81d (arm64: kernel: add code to set cpu boot mode
to secondary_entry shim) we no longer compute the phys-page offset
before el2_setup.

Where does the fault happen?

-- 
Catalin

Re: [PATCH RFC v3 08/12] arm64: kernel: implement HW breakpoints CPU PM notifier

[Will Deacon]

Hi Lorenzo,

On Thu, Nov 21, 2013 at 11:24:15AM +0000, Lorenzo Pieralisi wrote:
> When a CPU is shutdown either through CPU idle or suspend to RAM, the
> content of HW breakpoint registers must be reset or restored to proper
> values when CPU resume from low power states. This patch adds debug register
> restore operations to the HW breakpoint control function and implements a
> CPU PM notifier that allows to restore the content of HW breakpoint registers
> to allow proper suspend/resume operations.

This looks mostly fine to me, but I have one questions you might be able to
answer...

> @@ -840,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
>  /*
>   * CPU initialisation.
>   */
> -static void reset_ctrl_regs(void *unused)
> +static void hw_breakpoint_reset(void *unused)
>  {
>  	int i;
> -
> -	for (i = 0; i < core_num_brps; ++i) {
> -		write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> -		write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> +	struct perf_event **slots;
> +	/*
> +	 * When a CPU goes through cold-boot, it does not have any installed
> +	 * slot, so it is safe to share the same function for restoring and
> +	 * resetting breakpoints; when a CPU is hotplugged in, it goes
> +	 * through the slots, which are all empty, hence it just resets control
> +	 * and value for debug registers.
> +	 * When this function is triggered on warm-boot through a CPU PM
> +	 * notifier some slots might be initialized; if so they are
> +	 * reprogrammed according to the debug slots content.
> +	 */
> +	for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
> +		if (slots[i]) {
> +			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
> +		} else {
> +			write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> +			write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> +		}

When this runs on warm-boot and starts restoring debug state, are debug
exceptions guaranteed to be masked? I think that the debug restoration
should appear atomic to a debugger (i.e. you can't take a debug exception
half-way through the restore).

Providing that's the case:

  Acked-by: Will Deacon <will.deacon@arm.com>

Will

[PATCH RFC v3 08/12] arm64: kernel: implement HW breakpoints CPU PM notifier

[Will Deacon]

Hi Lorenzo,

On Thu, Nov 21, 2013 at 11:24:15AM +0000, Lorenzo Pieralisi wrote:
> When a CPU is shutdown either through CPU idle or suspend to RAM, the
> content of HW breakpoint registers must be reset or restored to proper
> values when CPU resume from low power states. This patch adds debug register
> restore operations to the HW breakpoint control function and implements a
> CPU PM notifier that allows to restore the content of HW breakpoint registers
> to allow proper suspend/resume operations.

This looks mostly fine to me, but I have one questions you might be able to
answer...

> @@ -840,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
>  /*
>   * CPU initialisation.
>   */
> -static void reset_ctrl_regs(void *unused)
> +static void hw_breakpoint_reset(void *unused)
>  {
>  	int i;
> -
> -	for (i = 0; i < core_num_brps; ++i) {
> -		write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> -		write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> +	struct perf_event **slots;
> +	/*
> +	 * When a CPU goes through cold-boot, it does not have any installed
> +	 * slot, so it is safe to share the same function for restoring and
> +	 * resetting breakpoints; when a CPU is hotplugged in, it goes
> +	 * through the slots, which are all empty, hence it just resets control
> +	 * and value for debug registers.
> +	 * When this function is triggered on warm-boot through a CPU PM
> +	 * notifier some slots might be initialized; if so they are
> +	 * reprogrammed according to the debug slots content.
> +	 */
> +	for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
> +		if (slots[i]) {
> +			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
> +		} else {
> +			write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> +			write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> +		}

When this runs on warm-boot and starts restoring debug state, are debug
exceptions guaranteed to be masked? I think that the debug restoration
should appear atomic to a debugger (i.e. you can't take a debug exception
half-way through the restore).

Providing that's the case:

  Acked-by: Will Deacon <will.deacon@arm.com>

Will

Re: [PATCH RFC v3 08/12] arm64: kernel: implement HW breakpoints CPU PM notifier

[Lorenzo Pieralisi]

Hi Will,

thanks for having a look.

On Fri, Dec 20, 2013 at 05:29:07PM +0000, Will Deacon wrote:
> Hi Lorenzo,
> 
> On Thu, Nov 21, 2013 at 11:24:15AM +0000, Lorenzo Pieralisi wrote:
> > When a CPU is shutdown either through CPU idle or suspend to RAM, the
> > content of HW breakpoint registers must be reset or restored to proper
> > values when CPU resume from low power states. This patch adds debug register
> > restore operations to the HW breakpoint control function and implements a
> > CPU PM notifier that allows to restore the content of HW breakpoint registers
> > to allow proper suspend/resume operations.
> 
> This looks mostly fine to me, but I have one questions you might be able to
> answer...
> 
> > @@ -840,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
> >  /*
> >   * CPU initialisation.
> >   */
> > -static void reset_ctrl_regs(void *unused)
> > +static void hw_breakpoint_reset(void *unused)
> >  {
> >  	int i;
> > -
> > -	for (i = 0; i < core_num_brps; ++i) {
> > -		write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> > -		write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> > +	struct perf_event **slots;
> > +	/*
> > +	 * When a CPU goes through cold-boot, it does not have any installed
> > +	 * slot, so it is safe to share the same function for restoring and
> > +	 * resetting breakpoints; when a CPU is hotplugged in, it goes
> > +	 * through the slots, which are all empty, hence it just resets control
> > +	 * and value for debug registers.
> > +	 * When this function is triggered on warm-boot through a CPU PM
> > +	 * notifier some slots might be initialized; if so they are
> > +	 * reprogrammed according to the debug slots content.
> > +	 */
> > +	for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
> > +		if (slots[i]) {
> > +			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
> > +		} else {
> > +			write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> > +			write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> > +		}
> 
> When this runs on warm-boot and starts restoring debug state, are debug
> exceptions guaranteed to be masked? I think that the debug restoration
> should appear atomic to a debugger (i.e. you can't take a debug exception
> half-way through the restore).

No, they might not be, good catch and this needs fixing (well, CPU PM notifiers
and suspend must be run with IRQs disabled, but debug exceptions can be left
on even though we should define to what extent this can be considered
reasonable/safe, eg debugging should not fiddle with HW state that we are
saving, it is a bit of a grey area).
I think the easiest option is to mask/unmask them in the CPU PM notifier,
before/after restoring the slots. As we discussed, I am trying to keep debug
on as deep in the power down sequence as possible, and now I mask debug
exceptions before saving general purpose registers, and restore the exception
flags when cpu_suspend returns (which means unfortunately that the sequence
above might run with debug exceptions unmasked). This means that the debugger
might see a restore that is not atomic from its perspective, again, good spot,
I should update the patch or put together a fix for -rc1 if the commit history
is immutable now, which I think that's the case.

Thank you,
Lorenzo

> 
> Providing that's the case:
> 
>   Acked-by: Will Deacon <will.deacon@arm.com>
> 
> Will
> --
> To unsubscribe from this list: send the line "unsubscribe linux-pm" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html
> 

[PATCH RFC v3 08/12] arm64: kernel: implement HW breakpoints CPU PM notifier

[Lorenzo Pieralisi]

Hi Will,

thanks for having a look.

On Fri, Dec 20, 2013 at 05:29:07PM +0000, Will Deacon wrote:
> Hi Lorenzo,
> 
> On Thu, Nov 21, 2013 at 11:24:15AM +0000, Lorenzo Pieralisi wrote:
> > When a CPU is shutdown either through CPU idle or suspend to RAM, the
> > content of HW breakpoint registers must be reset or restored to proper
> > values when CPU resume from low power states. This patch adds debug register
> > restore operations to the HW breakpoint control function and implements a
> > CPU PM notifier that allows to restore the content of HW breakpoint registers
> > to allow proper suspend/resume operations.
> 
> This looks mostly fine to me, but I have one questions you might be able to
> answer...
> 
> > @@ -840,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
> >  /*
> >   * CPU initialisation.
> >   */
> > -static void reset_ctrl_regs(void *unused)
> > +static void hw_breakpoint_reset(void *unused)
> >  {
> >  	int i;
> > -
> > -	for (i = 0; i < core_num_brps; ++i) {
> > -		write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> > -		write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> > +	struct perf_event **slots;
> > +	/*
> > +	 * When a CPU goes through cold-boot, it does not have any installed
> > +	 * slot, so it is safe to share the same function for restoring and
> > +	 * resetting breakpoints; when a CPU is hotplugged in, it goes
> > +	 * through the slots, which are all empty, hence it just resets control
> > +	 * and value for debug registers.
> > +	 * When this function is triggered on warm-boot through a CPU PM
> > +	 * notifier some slots might be initialized; if so they are
> > +	 * reprogrammed according to the debug slots content.
> > +	 */
> > +	for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
> > +		if (slots[i]) {
> > +			hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
> > +		} else {
> > +			write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
> > +			write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
> > +		}
> 
> When this runs on warm-boot and starts restoring debug state, are debug
> exceptions guaranteed to be masked? I think that the debug restoration
> should appear atomic to a debugger (i.e. you can't take a debug exception
> half-way through the restore).

No, they might not be, good catch and this needs fixing (well, CPU PM notifiers
and suspend must be run with IRQs disabled, but debug exceptions can be left
on even though we should define to what extent this can be considered
reasonable/safe, eg debugging should not fiddle with HW state that we are
saving, it is a bit of a grey area).
I think the easiest option is to mask/unmask them in the CPU PM notifier,
before/after restoring the slots. As we discussed, I am trying to keep debug
on as deep in the power down sequence as possible, and now I mask debug
exceptions before saving general purpose registers, and restore the exception
flags when cpu_suspend returns (which means unfortunately that the sequence
above might run with debug exceptions unmasked). This means that the debugger
might see a restore that is not atomic from its perspective, again, good spot,
I should update the patch or put together a fix for -rc1 if the commit history
is immutable now, which I think that's the case.

Thank you,
Lorenzo

> 
> Providing that's the case:
> 
>   Acked-by: Will Deacon <will.deacon@arm.com>
> 
> Will
> --
> To unsubscribe from this list: send the line "unsubscribe linux-pm" in
> the body of a message to majordomo at vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html
> 

Re: [PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Lorenzo Pieralisi]

On Fri, Dec 20, 2013 at 11:30:16AM +0000, Leo Yan wrote:
> On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:
> 
> > +/*
> > + * x0 must contain the sctlr value retrieved from restored context
> > + */
> > +ENTRY(cpu_resume_mmu)
> > +	ldr	x3, =cpu_resume_after_mmu
> > +	msr	sctlr_el1, x0		// restore sctlr_el1
> > +	isb
> > +	br	x3			// global jump to virtual address
> > +ENDPROC(cpu_resume_mmu)
> > +cpu_resume_after_mmu:
> > +	mov	x0, #0			// return zero on success
> > +	ldp	x19, x20, [sp, #16]
> > +	ldp	x21, x22, [sp, #32]
> > +	ldp	x23, x24, [sp, #48]
> > +	ldp	x25, x26, [sp, #64]
> > +	ldp	x27, x28, [sp, #80]
> > +	ldp	x29, lr, [sp], #96
> > +	ret
> > +ENDPROC(cpu_resume_after_mmu)
> > +
> > +	.data
> > +ENTRY(cpu_resume)
> > +	bl	el2_setup		// if in EL2 drop to EL1 cleanly
> 
> Compare to v2's patch set, here remove the calculation fro the offset 
> b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28 
> is zero and finally introduce the EL2's sync exception. Below are pasted 
> v2's code for reference.

What kernel are you testing against ? The offset is not needed anymore
in el2_setup, that is why the x28 computation is not there.

> do u want use firmware to set the x28 for the offset value? :-) IMHO, 
> v2's implementation is more reasonable and it's better keep the code.

The point is not whether to set it in firmware or in the kernel, see above.

Lorenzo

> 
> ENTRY(cpu_resume)
>            adr     x4, sleep_save_sp
>            ldr     x5, =sleep_save_sp
>            sub     x28, x4, x5             // x28 = PHYS_OFFSET - 
> PAGE_OFFSET
>            /*
>             * make sure el2 is sane, el2_setup expects:
>             * x28 = PHYS_OFFSET - PAGE_OFFSET
>             */
>            bl      el2_setup               // if in EL2 drop to EL1 cleanly
> 
> 
> > +#ifdef CONFIG_SMP
> > +	mrs	x1, mpidr_el1
> > +	adr	x4, mpidr_hash_ptr
> > +	ldr	x5, [x4]
> > +	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
> > +        /* retrieve mpidr_hash members to compute the hash */
> > +	ldr	x2, [x8, #MPIDR_HASH_MASK]
> > +	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
> > +	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
> > +	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
> > +        /* x7 contains hash index, let's use it to grab context pointer */
> > +#else
> > +	mov	x7, xzr
> > +#endif
> > +	adr	x0, sleep_save_sp
> > +	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
> > +	ldr	x0, [x0, x7, lsl #3]
> > +	/* load sp from context */
> > +	ldr	x2, [x0, #CPU_CTX_SP]
> > +	adr	x1, sleep_idmap_phys
> > +	/* load physical address of identity map page table in x1 */
> > +	ldr	x1, [x1]
> > +	mov	sp, x2
> > +	/*
> > +	 * cpu_do_resume expects x0 to contain context physical address
> > +	 * pointer and x1 to contain physical address of 1:1 page tables
> > +	 */
> > +	bl	cpu_do_resume		// PC relative jump, MMU off
> > +	b	cpu_resume_mmu		// Resume MMU, never returns
> > +ENDPROC(cpu_resume)
> > +
> 

[PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Lorenzo Pieralisi]

On Fri, Dec 20, 2013 at 11:30:16AM +0000, Leo Yan wrote:
> On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:
> 
> > +/*
> > + * x0 must contain the sctlr value retrieved from restored context
> > + */
> > +ENTRY(cpu_resume_mmu)
> > +	ldr	x3, =cpu_resume_after_mmu
> > +	msr	sctlr_el1, x0		// restore sctlr_el1
> > +	isb
> > +	br	x3			// global jump to virtual address
> > +ENDPROC(cpu_resume_mmu)
> > +cpu_resume_after_mmu:
> > +	mov	x0, #0			// return zero on success
> > +	ldp	x19, x20, [sp, #16]
> > +	ldp	x21, x22, [sp, #32]
> > +	ldp	x23, x24, [sp, #48]
> > +	ldp	x25, x26, [sp, #64]
> > +	ldp	x27, x28, [sp, #80]
> > +	ldp	x29, lr, [sp], #96
> > +	ret
> > +ENDPROC(cpu_resume_after_mmu)
> > +
> > +	.data
> > +ENTRY(cpu_resume)
> > +	bl	el2_setup		// if in EL2 drop to EL1 cleanly
> 
> Compare to v2's patch set, here remove the calculation fro the offset 
> b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28 
> is zero and finally introduce the EL2's sync exception. Below are pasted 
> v2's code for reference.

What kernel are you testing against ? The offset is not needed anymore
in el2_setup, that is why the x28 computation is not there.

> do u want use firmware to set the x28 for the offset value? :-) IMHO, 
> v2's implementation is more reasonable and it's better keep the code.

The point is not whether to set it in firmware or in the kernel, see above.

Lorenzo

> 
> ENTRY(cpu_resume)
>            adr     x4, sleep_save_sp
>            ldr     x5, =sleep_save_sp
>            sub     x28, x4, x5             // x28 = PHYS_OFFSET - 
> PAGE_OFFSET
>            /*
>             * make sure el2 is sane, el2_setup expects:
>             * x28 = PHYS_OFFSET - PAGE_OFFSET
>             */
>            bl      el2_setup               // if in EL2 drop to EL1 cleanly
> 
> 
> > +#ifdef CONFIG_SMP
> > +	mrs	x1, mpidr_el1
> > +	adr	x4, mpidr_hash_ptr
> > +	ldr	x5, [x4]
> > +	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
> > +        /* retrieve mpidr_hash members to compute the hash */
> > +	ldr	x2, [x8, #MPIDR_HASH_MASK]
> > +	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
> > +	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
> > +	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
> > +        /* x7 contains hash index, let's use it to grab context pointer */
> > +#else
> > +	mov	x7, xzr
> > +#endif
> > +	adr	x0, sleep_save_sp
> > +	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
> > +	ldr	x0, [x0, x7, lsl #3]
> > +	/* load sp from context */
> > +	ldr	x2, [x0, #CPU_CTX_SP]
> > +	adr	x1, sleep_idmap_phys
> > +	/* load physical address of identity map page table in x1 */
> > +	ldr	x1, [x1]
> > +	mov	sp, x2
> > +	/*
> > +	 * cpu_do_resume expects x0 to contain context physical address
> > +	 * pointer and x1 to contain physical address of 1:1 page tables
> > +	 */
> > +	bl	cpu_do_resume		// PC relative jump, MMU off
> > +	b	cpu_resume_mmu		// Resume MMU, never returns
> > +ENDPROC(cpu_resume)
> > +
> 

Re: [PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Leo Yan]

On 12/23/2013 10:04 PM, Lorenzo Pieralisi wrote:
> On Fri, Dec 20, 2013 at 11:30:16AM +0000, Leo Yan wrote:
>> On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:
>>
>>> +/*
>>> + * x0 must contain the sctlr value retrieved from restored context
>>> + */
>>> +ENTRY(cpu_resume_mmu)
>>> +	ldr	x3, =cpu_resume_after_mmu
>>> +	msr	sctlr_el1, x0		// restore sctlr_el1
>>> +	isb
>>> +	br	x3			// global jump to virtual address
>>> +ENDPROC(cpu_resume_mmu)
>>> +cpu_resume_after_mmu:
>>> +	mov	x0, #0			// return zero on success
>>> +	ldp	x19, x20, [sp, #16]
>>> +	ldp	x21, x22, [sp, #32]
>>> +	ldp	x23, x24, [sp, #48]
>>> +	ldp	x25, x26, [sp, #64]
>>> +	ldp	x27, x28, [sp, #80]
>>> +	ldp	x29, lr, [sp], #96
>>> +	ret
>>> +ENDPROC(cpu_resume_after_mmu)
>>> +
>>> +	.data
>>> +ENTRY(cpu_resume)
>>> +	bl	el2_setup		// if in EL2 drop to EL1 cleanly
>>
>> Compare to v2's patch set, here remove the calculation fro the offset
>> b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28
>> is zero and finally introduce the EL2's sync exception. Below are pasted
>> v2's code for reference.
>
> What kernel are you testing against ? The offset is not needed anymore
> in el2_setup, that is why the x28 computation is not there.
>
>> do u want use firmware to set the x28 for the offset value? :-) IMHO,
>> v2's implementation is more reasonable and it's better keep the code.
>
> The point is not whether to set it in firmware or in the kernel, see above.
>
> Lorenzo
>

Catalin and Lorenzo, Thanks a lot for u point out.

Now i'm back porting patches to 3.10, but the cpu_suspend/resume patches 
are dependent on BE related patches on the latest mainline code base. 
After apply below missing patches, now the cpu_suspend/resume can work 
well. :-)

arm64: asm: add CPU_LE & CPU_BE assembler helpers
arm64: head: create a new function for setting the boot_cpu_mode flag
arm64: big-endian: set correct endianess on kernel entry
arm64: kernel: add code to set cpu boot mode to secondary_entry shim

>>
>> ENTRY(cpu_resume)
>>             adr     x4, sleep_save_sp
>>             ldr     x5, =sleep_save_sp
>>             sub     x28, x4, x5             // x28 = PHYS_OFFSET -
>> PAGE_OFFSET
>>             /*
>>              * make sure el2 is sane, el2_setup expects:
>>              * x28 = PHYS_OFFSET - PAGE_OFFSET
>>              */
>>             bl      el2_setup               // if in EL2 drop to EL1 cleanly
>>
>>
>>> +#ifdef CONFIG_SMP
>>> +	mrs	x1, mpidr_el1
>>> +	adr	x4, mpidr_hash_ptr
>>> +	ldr	x5, [x4]
>>> +	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
>>> +        /* retrieve mpidr_hash members to compute the hash */
>>> +	ldr	x2, [x8, #MPIDR_HASH_MASK]
>>> +	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
>>> +	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
>>> +	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
>>> +        /* x7 contains hash index, let's use it to grab context pointer */
>>> +#else
>>> +	mov	x7, xzr
>>> +#endif
>>> +	adr	x0, sleep_save_sp
>>> +	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
>>> +	ldr	x0, [x0, x7, lsl #3]
>>> +	/* load sp from context */
>>> +	ldr	x2, [x0, #CPU_CTX_SP]
>>> +	adr	x1, sleep_idmap_phys
>>> +	/* load physical address of identity map page table in x1 */
>>> +	ldr	x1, [x1]
>>> +	mov	sp, x2
>>> +	/*
>>> +	 * cpu_do_resume expects x0 to contain context physical address
>>> +	 * pointer and x1 to contain physical address of 1:1 page tables
>>> +	 */
>>> +	bl	cpu_do_resume		// PC relative jump, MMU off
>>> +	b	cpu_resume_mmu		// Resume MMU, never returns
>>> +ENDPROC(cpu_resume)
>>> +
>>
>

[PATCH RFC v3 04/12] arm64: kernel: cpu_{suspend/resume} implementation

[Leo Yan]

On 12/23/2013 10:04 PM, Lorenzo Pieralisi wrote:
> On Fri, Dec 20, 2013 at 11:30:16AM +0000, Leo Yan wrote:
>> On 11/21/2013 07:24 PM, Lorenzo Pieralisi wrote:
>>
>>> +/*
>>> + * x0 must contain the sctlr value retrieved from restored context
>>> + */
>>> +ENTRY(cpu_resume_mmu)
>>> +	ldr	x3, =cpu_resume_after_mmu
>>> +	msr	sctlr_el1, x0		// restore sctlr_el1
>>> +	isb
>>> +	br	x3			// global jump to virtual address
>>> +ENDPROC(cpu_resume_mmu)
>>> +cpu_resume_after_mmu:
>>> +	mov	x0, #0			// return zero on success
>>> +	ldp	x19, x20, [sp, #16]
>>> +	ldp	x21, x22, [sp, #32]
>>> +	ldp	x23, x24, [sp, #48]
>>> +	ldp	x25, x26, [sp, #64]
>>> +	ldp	x27, x28, [sp, #80]
>>> +	ldp	x29, lr, [sp], #96
>>> +	ret
>>> +ENDPROC(cpu_resume_after_mmu)
>>> +
>>> +	.data
>>> +ENTRY(cpu_resume)
>>> +	bl	el2_setup		// if in EL2 drop to EL1 cleanly
>>
>> Compare to v2's patch set, here remove the calculation fro the offset
>> b/t PHYS_OFFSET - PAGE_OFFSET; so when i verify the patch set, i saw x28
>> is zero and finally introduce the EL2's sync exception. Below are pasted
>> v2's code for reference.
>
> What kernel are you testing against ? The offset is not needed anymore
> in el2_setup, that is why the x28 computation is not there.
>
>> do u want use firmware to set the x28 for the offset value? :-) IMHO,
>> v2's implementation is more reasonable and it's better keep the code.
>
> The point is not whether to set it in firmware or in the kernel, see above.
>
> Lorenzo
>

Catalin and Lorenzo, Thanks a lot for u point out.

Now i'm back porting patches to 3.10, but the cpu_suspend/resume patches 
are dependent on BE related patches on the latest mainline code base. 
After apply below missing patches, now the cpu_suspend/resume can work 
well. :-)

arm64: asm: add CPU_LE & CPU_BE assembler helpers
arm64: head: create a new function for setting the boot_cpu_mode flag
arm64: big-endian: set correct endianess on kernel entry
arm64: kernel: add code to set cpu boot mode to secondary_entry shim

>>
>> ENTRY(cpu_resume)
>>             adr     x4, sleep_save_sp
>>             ldr     x5, =sleep_save_sp
>>             sub     x28, x4, x5             // x28 = PHYS_OFFSET -
>> PAGE_OFFSET
>>             /*
>>              * make sure el2 is sane, el2_setup expects:
>>              * x28 = PHYS_OFFSET - PAGE_OFFSET
>>              */
>>             bl      el2_setup               // if in EL2 drop to EL1 cleanly
>>
>>
>>> +#ifdef CONFIG_SMP
>>> +	mrs	x1, mpidr_el1
>>> +	adr	x4, mpidr_hash_ptr
>>> +	ldr	x5, [x4]
>>> +	add	x8, x4, x5		// x8 = struct mpidr_hash phys address
>>> +        /* retrieve mpidr_hash members to compute the hash */
>>> +	ldr	x2, [x8, #MPIDR_HASH_MASK]
>>> +	ldp	w3, w4, [x8, #MPIDR_HASH_SHIFTS]
>>> +	ldp	w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
>>> +	compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
>>> +        /* x7 contains hash index, let's use it to grab context pointer */
>>> +#else
>>> +	mov	x7, xzr
>>> +#endif
>>> +	adr	x0, sleep_save_sp
>>> +	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
>>> +	ldr	x0, [x0, x7, lsl #3]
>>> +	/* load sp from context */
>>> +	ldr	x2, [x0, #CPU_CTX_SP]
>>> +	adr	x1, sleep_idmap_phys
>>> +	/* load physical address of identity map page table in x1 */
>>> +	ldr	x1, [x1]
>>> +	mov	sp, x2
>>> +	/*
>>> +	 * cpu_do_resume expects x0 to contain context physical address
>>> +	 * pointer and x1 to contain physical address of 1:1 page tables
>>> +	 */
>>> +	bl	cpu_do_resume		// PC relative jump, MMU off
>>> +	b	cpu_resume_mmu		// Resume MMU, never returns
>>> +ENDPROC(cpu_resume)
>>> +
>>
>