powerpc: Merge smp-tbsync.c (the generic timebase sync routine)

Signed-off-by: Paul Mackerras <paulus@samba.org>

3 files changed, 179 insertions, 0 deletions

diff --git a/arch/powerpc/Kconfig b/arch/powerpc/Kconfig
index 3cf03ab46113..f4e25c648fbb 100644
--- a/arch/powerpc/Kconfig
+++ b/arch/powerpc/Kconfig
@@ -300,6 +300,7 @@ config PPC_PMAC64
         bool
         depends on PPC_PMAC && POWER4
         select U3_DART
+        select GENERIC_TBSYNC
         default y
 
 config PPC_PREP
@@ -314,6 +315,7 @@ config PPC_MAPLE
         bool "  Maple 970FX Evaluation Board"
         select U3_DART
         select MPIC_BROKEN_U3
+        select GENERIC_TBSYNC
         default n
         help
           This option enables support for the Maple 970FX Evaluation Board.
@@ -386,6 +388,11 @@ config PPC_MPC106
         bool
         default n
 
+config GENERIC_TBSYNC
+        bool
+        default y if CONFIG_PPC32 && CONFIG_SMP
+        default n
+
 source "drivers/cpufreq/Kconfig"
 
 config CPU_FREQ_PMAC
diff --git a/arch/powerpc/kernel/Makefile b/arch/powerpc/kernel/Makefile
index 7a3e1155ac9a..631149ea93db 100644
--- a/arch/powerpc/kernel/Makefile
+++ b/arch/powerpc/kernel/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_PPC_RTAS)		+= rtas.o
 obj-$(CONFIG_RTAS_FLASH)        += rtas_flash.o
 obj-$(CONFIG_RTAS_PROC)         += rtas-proc.o
 obj-$(CONFIG_IBMVIO)            += vio.o
+obj-$(CONFIG_GENERIC_TBSYNC)    += smp-tbsync.o
 
 ifeq ($(CONFIG_PPC_MERGE),y)
 
diff --git a/arch/powerpc/kernel/smp-tbsync.c b/arch/powerpc/kernel/smp-tbsync.c
new file mode 100644
index 000000000000..9adef3bddad4
--- /dev/null
+++ b/arch/powerpc/kernel/smp-tbsync.c
@@ -0,0 +1,171 @@
+/*
+ * Smp timebase synchronization for ppc.
+ *
+ * Copyright (C) 2003 Samuel Rydh (samuel@ibrium.se)
+ *
+ */
+
+#include <linux/config.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+#include <linux/unistd.h>
+#include <linux/init.h>
+#include <asm/atomic.h>
+#include <asm/smp.h>
+#include <asm/time.h>
+
+#define NUM_ITER                300
+
+enum {
+        kExit=0, kSetAndTest, kTest
+};
+
+static struct {
+        volatile u64            tb;
+        volatile u64            mark;
+        volatile int            cmd;
+        volatile int            handshake;
+        int                     filler[2];
+
+        volatile int            ack;
+        int                     filler2[7];
+
+        volatile int            race_result;
+} *tbsync;
+
+static volatile int             running;
+
+static void __devinit enter_contest(u64 mark, long add)
+{
+        while (get_tb() < mark)
+                tbsync->race_result = add;
+}
+
+void __devinit smp_generic_take_timebase(void)
+{
+        int cmd;
+        u64 tb;
+
+        local_irq_disable();
+        while (!running)
+                barrier();
+        rmb();
+
+        for (;;) {
+                tbsync->ack = 1;
+                while (!tbsync->handshake)
+                        barrier();
+                rmb();
+
+                cmd = tbsync->cmd;
+                tb = tbsync->tb;
+                mb();
+                tbsync->ack = 0;
+                if (cmd == kExit)
+                        break;
+
+                while (tbsync->handshake)
+                        barrier();
+                if (cmd == kSetAndTest)
+                        set_tb(tb >> 32, tb & 0xfffffffful);
+                enter_contest(tbsync->mark, -1);
+        }
+        local_irq_enable();
+}
+
+static int __devinit start_contest(int cmd, long offset, int num)
+{
+        int i, score=0;
+        u64 tb;
+        long mark;
+
+        tbsync->cmd = cmd;
+
+        local_irq_disable();
+        for (i = -3; i < num; ) {
+                tb = get_tb() + 400;
+                tbsync->tb = tb + offset;
+                tbsync->mark = mark = tb + 400;
+
+                wmb();
+
+                tbsync->handshake = 1;
+                while (tbsync->ack)
+                        barrier();
+
+                while (get_tb() <= tb)
+                        barrier();
+                tbsync->handshake = 0;
+                enter_contest(mark, 1);
+
+                while (!tbsync->ack)
+                        barrier();
+
+                if (i++ > 0)
+                        score += tbsync->race_result;
+        }
+        local_irq_enable();
+        return score;
+}
+
+void __devinit smp_generic_give_timebase(void)
+{
+        int i, score, score2, old, min=0, max=5000, offset=1000;
+
+        printk("Synchronizing timebase\n");
+
+        /* if this fails then this kernel won't work anyway... */
+        tbsync = kmalloc( sizeof(*tbsync), GFP_KERNEL );
+        memset( tbsync, 0, sizeof(*tbsync) );
+        mb();
+        running = 1;
+
+        while (!tbsync->ack)
+                barrier();
+
+        printk("Got ack\n");
+
+        /* binary search */
+        for (old = -1; old != offset ; offset = (min+max) / 2) {
+                score = start_contest(kSetAndTest, offset, NUM_ITER);
+
+                printk("score %d, offset %d\n", score, offset );
+
+                if( score > 0 )
+                        max = offset;
+                else
+                        min = offset;
+                old = offset;
+        }
+        score = start_contest(kSetAndTest, min, NUM_ITER);
+        score2 = start_contest(kSetAndTest, max, NUM_ITER);
+
+        printk("Min %d (score %d), Max %d (score %d)\n",
+               min, score, max, score2);
+        score = abs(score);
+        score2 = abs(score2);
+        offset = (score < score2) ? min : max;
+
+        /* guard against inaccurate mttb */
+        for (i = 0; i < 10; i++) {
+                start_contest(kSetAndTest, offset, NUM_ITER/10);
+
+                if ((score2 = start_contest(kTest, offset, NUM_ITER)) < 0)
+                        score2 = -score2;
+                if (score2 <= score || score2 < 20)
+                        break;
+        }
+        printk("Final offset: %d (%d/%d)\n", offset, score2, NUM_ITER );
+
+        /* exiting */
+        tbsync->cmd = kExit;
+        wmb();
+        tbsync->handshake = 1;
+        while (tbsync->ack)
+                barrier();
+        tbsync->handshake = 0;
+        kfree(tbsync);
+        tbsync = NULL;
+        running = 0;
+}