Merge branch 'mpidr-updates-for-rmk' of git://linux-arm.org/linux-2.6-lp into devel-stable

This patch series that implements MPIDR linearization through a simple
hashing algorithm and updates current cpu_{suspend}/{resume} code to use
the newly created hash structures to retrieve context pointers.  It
represents a stepping stone for the implementation of power management code
on forthcoming multi-cluster ARM systems.

It has been tested on TC2 (dual cluster A15xA7 system), iMX6q, OMAP4 and
Tegra, with processors hitting low-power states requiring warm-boot resume
through the cpu_resume code path.

6 files changed, 195 insertions, 18 deletions

diff --git a/arch/arm/include/asm/smp_plat.h b/arch/arm/include/asm/smp_plat.h
index 1c7b6f8101ae..6e63f29f41b7 100644
--- a/arch/arm/include/asm/smp_plat.h
+++ b/arch/arm/include/asm/smp_plat.h
@@ -70,4 +70,22 @@ static inline int get_logical_index(u32 mpidr)
         return -EINVAL;
 }
 
+/*
+ * NOTE ! Assembly code relies on the following
+ * structure memory layout in order to carry out load
+ * multiple from its base address. For more
+ * information check arch/arm/kernel/sleep.S
+ */
+struct mpidr_hash {
+        u32     mask; /* used by sleep.S */
+        u32     shift_aff[3]; /* used by sleep.S */
+        u32     bits;
+};
+
+extern struct mpidr_hash mpidr_hash;
+
+static inline u32 mpidr_hash_size(void)
+{
+        return 1 << mpidr_hash.bits;
+}
 #endif
diff --git a/arch/arm/include/asm/suspend.h b/arch/arm/include/asm/suspend.h
index 1c0a551ae375..cd20029bcd94 100644
--- a/arch/arm/include/asm/suspend.h
+++ b/arch/arm/include/asm/suspend.h
@@ -1,6 +1,11 @@
 #ifndef __ASM_ARM_SUSPEND_H
 #define __ASM_ARM_SUSPEND_H
 
+struct sleep_save_sp {
+        u32 *save_ptr_stash;
+        u32 save_ptr_stash_phys;
+};
+
 extern void cpu_resume(void);
 extern int cpu_suspend(unsigned long, int (*)(unsigned long));
 
diff --git a/arch/arm/kernel/asm-offsets.c b/arch/arm/kernel/asm-offsets.c
index ee68cce6b48e..ded041711beb 100644
--- a/arch/arm/kernel/asm-offsets.c
+++ b/arch/arm/kernel/asm-offsets.c
@@ -23,6 +23,7 @@
 #include <asm/thread_info.h>
 #include <asm/memory.h>
 #include <asm/procinfo.h>
+#include <asm/suspend.h>
 #include <asm/hardware/cache-l2x0.h>
 #include <linux/kbuild.h>
 
@@ -145,6 +146,11 @@ int main(void)
 #ifdef MULTI_CACHE
   DEFINE(CACHE_FLUSH_KERN_ALL,  offsetof(struct cpu_cache_fns, flush_kern_all));
 #endif
+#ifdef CONFIG_ARM_CPU_SUSPEND
+  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
   BLANK();
   DEFINE(DMA_BIDIRECTIONAL,     DMA_BIDIRECTIONAL);
   DEFINE(DMA_TO_DEVICE,         DMA_TO_DEVICE);
diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c
index ca34224f891f..9048513cbe0d 100644
--- a/arch/arm/kernel/setup.c
+++ b/arch/arm/kernel/setup.c
@@ -478,6 +478,72 @@ void __init smp_setup_processor_id(void)
         printk(KERN_INFO "Booting Linux on physical CPU 0x%x\n", mpidr);
 }
 
+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;
+        u32 fs[3], bits[3], ls, 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 0x%x\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 < 3; 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 by isolating the
+         * significant bits at each affinity level and by shifting
+         * them in order to compress the 24 bits values space to a
+         * compressed set of values. This is equivalent to hashing
+         * the MPIDR 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[7:0] = {0x2, 0x80}.
+         */
+        mpidr_hash.shift_aff[0] = fs[0];
+        mpidr_hash.shift_aff[1] = MPIDR_LEVEL_BITS + fs[1] - bits[0];
+        mpidr_hash.shift_aff[2] = 2*MPIDR_LEVEL_BITS + fs[2] -
+                                                (bits[1] + bits[0]);
+        mpidr_hash.mask = mask;
+        mpidr_hash.bits = bits[2] + bits[1] + bits[0];
+        pr_debug("MPIDR hash: aff0[%u] aff1[%u] aff2[%u] mask[0x%x] bits[%u]\n",
+                                mpidr_hash.shift_aff[0],
+                                mpidr_hash.shift_aff[1],
+                                mpidr_hash.shift_aff[2],
+                                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");
+        sync_cache_w(&mpidr_hash);
+}
+#endif
+
 static void __init setup_processor(void)
 {
         struct proc_info_list *list;
@@ -825,6 +891,7 @@ void __init setup_arch(char **cmdline_p)
                                 smp_set_ops(mdesc->smp);
                 }
                 smp_init_cpus();
+                smp_build_mpidr_hash();
         }
 #endif
 
diff --git a/arch/arm/kernel/sleep.S b/arch/arm/kernel/sleep.S
index 987dcf33415c..db1536b8b30b 100644
--- a/arch/arm/kernel/sleep.S
+++ b/arch/arm/kernel/sleep.S
@@ -7,6 +7,49 @@
         .text
 
 /*
+ * Implementation of MPIDR 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
+ * @mpidr: register containing MPIDR value
+ * @mask: register containing MPIDR mask
+ *
+ * Pseudo C-code:
+ *
+ *u32 dst;
+ *
+ *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 mpidr, u32 mask) {
+ *      u32 aff0, aff1, aff2;
+ *      u32 mpidr_masked = mpidr & mask;
+ *      aff0 = mpidr_masked & 0xff;
+ *      aff1 = mpidr_masked & 0xff00;
+ *      aff2 = mpidr_masked & 0xff0000;
+ *      dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2);
+ *}
+ * Input registers: rs0, rs1, rs2, mpidr, mask
+ * Output register: dst
+ * Note: input and output registers must be disjoint register sets
+         (eg: a macro instance with mpidr = r1 and dst = r1 is invalid)
+ */
+        .macro compute_mpidr_hash dst, rs0, rs1, rs2, mpidr, mask
+        and     \mpidr, \mpidr, \mask                   @ mask out MPIDR bits
+        and     \dst, \mpidr, #0xff                     @ mask=aff0
+ ARM(   mov     \dst, \dst, lsr \rs0            )       @ dst=aff0>>rs0
+ THUMB( lsr     \dst, \dst, \rs0                )
+        and     \mask, \mpidr, #0xff00                  @ mask = aff1
+ ARM(   orr     \dst, \dst, \mask, lsr \rs1     )       @ dst|=(aff1>>rs1)
+ THUMB( lsr     \mask, \mask, \rs1              )
+ THUMB( orr     \dst, \dst, \mask               )
+        and     \mask, \mpidr, #0xff0000                @ mask = aff2
+ ARM(   orr     \dst, \dst, \mask, lsr \rs2     )       @ dst|=(aff2>>rs2)
+ THUMB( lsr     \mask, \mask, \rs2              )
+ THUMB( orr     \dst, \dst, \mask               )
+        .endm
+
+/*
  * Save CPU state for a suspend.  This saves the CPU general purpose
  * registers, and allocates space on the kernel stack to save the CPU
  * specific registers and some other data for resume.
@@ -29,12 +72,18 @@ ENTRY(__cpu_suspend)
         mov     r1, r4                  @ size of save block
         mov     r2, r5                  @ virtual SP
         ldr     r3, =sleep_save_sp
-#ifdef CONFIG_SMP
-        ALT_SMP(mrc p15, 0, lr, c0, c0, 5)
-        ALT_UP(mov lr, #0)
-        and     lr, lr, #15
+        ldr     r3, [r3, #SLEEP_SAVE_SP_VIRT]
+        ALT_SMP(mrc p15, 0, r9, c0, c0, 5)
+        ALT_UP_B(1f)
+        ldr     r8, =mpidr_hash
+        /*
+         * This ldmia relies on the memory layout of the mpidr_hash
+         * struct mpidr_hash.
+         */
+        ldmia   r8, {r4-r7}     @ r4 = mpidr mask (r5,r6,r7) = l[0,1,2] shifts
+        compute_mpidr_hash      lr, r5, r6, r7, r9, r4
         add     r3, r3, lr, lsl #2
-#endif
+1:
         bl      __cpu_suspend_save
         adr     lr, BSYM(cpu_suspend_abort)
         ldmfd   sp!, {r0, pc}           @ call suspend fn
@@ -81,15 +130,23 @@ ENDPROC(cpu_resume_after_mmu)
         .data
         .align
 ENTRY(cpu_resume)
-#ifdef CONFIG_SMP
-        adr     r0, sleep_save_sp
-        ALT_SMP(mrc p15, 0, r1, c0, c0, 5)
-        ALT_UP(mov r1, #0)
-        and     r1, r1, #15
-        ldr     r0, [r0, r1, lsl #2]    @ stack phys addr
-#else
-        ldr     r0, sleep_save_sp       @ stack phys addr
-#endif
+        mov     r1, #0
+        ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
+        ALT_UP_B(1f)
+        adr     r2, mpidr_hash_ptr
+        ldr     r3, [r2]
+        add     r2, r2, r3              @ r2 = struct mpidr_hash phys address
+        /*
+         * This ldmia relies on the memory layout of the mpidr_hash
+         * struct mpidr_hash.
+         */
+        ldmia   r2, { r3-r6 }   @ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
+        compute_mpidr_hash      r1, r4, r5, r6, r0, r3
+1:
+        adr     r0, _sleep_save_sp
+        ldr     r0, [r0, #SLEEP_SAVE_SP_PHYS]
+        ldr     r0, [r0, r1, lsl #2]
+
         setmode PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1  @ set SVC, irqs off
         @ load phys pgd, stack, resume fn
   ARM(  ldmia   r0!, {r1, sp, pc}       )
@@ -98,7 +155,11 @@ THUMB(	mov	sp, r2			)
 THUMB(  bx      r3                      )
 ENDPROC(cpu_resume)
 
-sleep_save_sp:
-        .rept   CONFIG_NR_CPUS
-        .long   0                               @ preserve stack phys ptr here
-        .endr
+        .align 2
+mpidr_hash_ptr:
+        .long   mpidr_hash - .                  @ mpidr_hash struct offset
+
+        .type   sleep_save_sp, #object
+ENTRY(sleep_save_sp)
+_sleep_save_sp:
+        .space  SLEEP_SAVE_SP_SZ                @ struct sleep_save_sp
diff --git a/arch/arm/kernel/suspend.c b/arch/arm/kernel/suspend.c
index 38a50676213b..41cf3cbf756d 100644
--- a/arch/arm/kernel/suspend.c
+++ b/arch/arm/kernel/suspend.c
@@ -1,9 +1,12 @@
 #include <linux/init.h>
+#include <linux/slab.h>
 
+#include <asm/cacheflush.h>
 #include <asm/idmap.h>
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
 #include <asm/memory.h>
+#include <asm/smp_plat.h>
 #include <asm/suspend.h>
 #include <asm/tlbflush.h>
 
@@ -82,3 +85,20 @@ void __cpu_suspend_save(u32 *ptr, u32 ptrsz, u32 sp, u32 *save_ptr)
         outer_clean_range(virt_to_phys(save_ptr),
                           virt_to_phys(save_ptr) + sizeof(*save_ptr));
 }
+
+extern struct sleep_save_sp sleep_save_sp;
+
+static int cpu_suspend_alloc_sp(void)
+{
+        void *ctx_ptr;
+        /* ctx_ptr is an array of physical addresses */
+        ctx_ptr = kcalloc(mpidr_hash_size(), sizeof(u32), 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);
+        sync_cache_w(&sleep_save_sp);
+        return 0;
+}
+early_initcall(cpu_suspend_alloc_sp);