1 files changed, 26 insertions, 0 deletions

diff --git a/arch/powerpc/platforms/85xx/smp.c b/arch/powerpc/platforms/85xx/smp.c
index 5ced4f5bb2b2..281b7f01df63 100644
--- a/arch/powerpc/platforms/85xx/smp.c
+++ b/arch/powerpc/platforms/85xx/smp.c
@@ -69,7 +69,32 @@ static void mpc85xx_give_timebase(void)
         tb_req = 0;
 
         mpc85xx_timebase_freeze(1);
+#ifdef CONFIG_PPC64
+        /*
+         * e5500/e6500 have a workaround for erratum A-006958 in place
+         * that will reread the timebase until TBL is non-zero.
+         * That would be a bad thing when the timebase is frozen.
+         *
+         * Thus, we read it manually, and instead of checking that
+         * TBL is non-zero, we ensure that TB does not change.  We don't
+         * do that for the main mftb implementation, because it requires
+         * a scratch register
+         */
+        {
+                u64 prev;
+
+                asm volatile("mfspr %0, %1" : "=r" (timebase) :
+                             "i" (SPRN_TBRL));
+
+                do {
+                        prev = timebase;
+                        asm volatile("mfspr %0, %1" : "=r" (timebase) :
+                                     "i" (SPRN_TBRL));
+                } while (prev != timebase);
+        }
+#else
         timebase = get_tb();
+#endif
         mb();
         tb_valid = 1;
 
@@ -255,6 +280,7 @@ out:
 
 struct smp_ops_t smp_85xx_ops = {
         .kick_cpu = smp_85xx_kick_cpu,
+        .cpu_bootable = smp_generic_cpu_bootable,
 #ifdef CONFIG_HOTPLUG_CPU
         .cpu_disable    = generic_cpu_disable,
         .cpu_die        = generic_cpu_die,