blackfin: bf60x: pm: Add a debug option to calculate kernel wakeup time.

Display the total time when kernel resumes normal from standby or suspend to mem
mode.

Signed-off-by: Sonic Zhang <sonic.zhang@analog.com>
Signed-off-by: Bob Liu <lliubbo@gmail.com>

6 files changed, 74 insertions, 0 deletions

diff --git a/arch/blackfin/Kconfig.debug b/arch/blackfin/Kconfig.debug
index e2a3d4c8ab9a..79594694ee90 100644
--- a/arch/blackfin/Kconfig.debug
+++ b/arch/blackfin/Kconfig.debug
@@ -253,4 +253,11 @@ config BFIN_PSEUDODBG_INSNS
 
           Most people should say N here.
 
+config BFIN_PM_WAKEUP_TIME_BENCH
+        bool "Display the total time for kernel to resume from power saving mode"
+        default n
+        help
+          Display the total time when kernel resumes normal from standby or
+          suspend to mem mode.
+
 endmenu
diff --git a/arch/blackfin/include/asm/dpmc.h b/arch/blackfin/include/asm/dpmc.h
index 528f47685781..e91eae8330a6 100644
--- a/arch/blackfin/include/asm/dpmc.h
+++ b/arch/blackfin/include/asm/dpmc.h
@@ -53,6 +53,16 @@
 #define PM_SYS_PUSH16(n, x)     _PM_PUSH(n, x, w, SYSMMR_BASE)
 #define PM_SYS_POP16(n, x)      _PM_POP(n, x, w, SYSMMR_BASE)
 
+        .macro bfin_init_pm_bench_cycles
+#ifdef CONFIG_BFIN_PM_WAKEUP_TIME_BENCH
+        R4 = 0;
+        CYCLES = R4;
+        CYCLES2 = R4;
+        R4 = SYSCFG;
+        BITSET(R4, 1);
+        SYSCFG = R4;
+#endif
+        .endm
 
         .macro bfin_cpu_reg_save
         /*
@@ -98,8 +108,10 @@
         r7 = RETI;
         [--sp] = RETS;
         [--sp] = ASTAT;
+#ifndef CONFIG_BFIN_PM_WAKEUP_TIME_BENCH
         [--sp] = CYCLES;
         [--sp] = CYCLES2;
+#endif
         [--sp] = SYSCFG;
         [--sp] = RETX;
         [--sp] = SEQSTAT;
@@ -115,8 +127,10 @@
         SEQSTAT = [sp++];
         RETX = [sp++];
         SYSCFG = [sp++];
+#ifndef CONFIG_BFIN_PM_WAKEUP_TIME_BENCH
         CYCLES2 = [sp++];
         CYCLES = [sp++];
+#endif
         ASTAT = [sp++];
         RETS = [sp++];
 
diff --git a/arch/blackfin/mach-bf609/hibernate.S b/arch/blackfin/mach-bf609/hibernate.S
index baedd6e6abf4..d37a532519c8 100644
--- a/arch/blackfin/mach-bf609/hibernate.S
+++ b/arch/blackfin/mach-bf609/hibernate.S
@@ -24,6 +24,9 @@ ENTRY(_enter_hibernate)
         P0.L = LO(DPM0_CTL);
         R3.H = HI(0x00000010);
         R3.L = LO(0x00000010);
+
+        bfin_init_pm_bench_cycles;
+
         [P0] = R3;
 
         SSYNC;
diff --git a/arch/blackfin/mach-bf609/pm.c b/arch/blackfin/mach-bf609/pm.c
index 79cdf41096c2..849d77e5631c 100644
--- a/arch/blackfin/mach-bf609/pm.c
+++ b/arch/blackfin/mach-bf609/pm.c
@@ -147,6 +147,18 @@ void bfin_deepsleep(unsigned long mask)
                         "idle;" \
                         : : \
                         );
+#ifdef CONFIG_BFIN_PM_WAKEUP_TIME_BENCH
+        __asm__ __volatile__(
+                "%0 = 0;"
+                "CYCLES = %0;"
+                "CYCLES2 = %0;"
+                "%0 = SYSCFG;"
+                "BITSET(%0, 1);"
+                "SYSCFG = %0;"
+                : "=d,a" (dpm0_ctl) :
+        );
+#endif
+
 }
 
 __attribute__((l1_text))
diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
index cfb7c3c00370..de99f3aac2c5 100644
--- a/arch/blackfin/mach-common/dpmc_modes.S
+++ b/arch/blackfin/mach-common/dpmc_modes.S
@@ -42,6 +42,9 @@ ENTRY(_sleep_mode)
         BITCLR (R7, 5);
         w[p0] = R7.L;
         IDLE;
+
+        bfin_init_pm_bench_cycles;
+
         call _test_pll_locked;
 
         RETS = [SP++];
@@ -74,6 +77,9 @@ ENTRY(_hibernate_mode)
 
         /* Finally, we climb into our cave to hibernate */
         W[P3] = R4.L;
+
+        bfin_init_pm_bench_cycles;
+
         CLI R2;
         IDLE;
 .Lforever:
@@ -158,6 +164,8 @@ ENTRY(_sleep_deeper)
         SSYNC;
         IDLE;
 
+        bfin_init_pm_bench_cycles;
+
         call _test_pll_locked;
 
         P0.H = hi(PLL_DIV);
diff --git a/arch/blackfin/mach-common/pm.c b/arch/blackfin/mach-common/pm.c
index 610a3cdf3998..ca6655e0d653 100644
--- a/arch/blackfin/mach-common/pm.c
+++ b/arch/blackfin/mach-common/pm.c
@@ -247,9 +247,39 @@ static int bfin_pm_enter(suspend_state_t state)
         return 0;
 }
 
+#ifdef CONFIG_BFIN_PM_WAKEUP_TIME_BENCH
+void bfin_pm_end(void)
+{
+        u32 cycle, cycle2;
+        u64 usec64;
+        u32 usec;
+
+        __asm__ __volatile__ (
+                "1: %0 = CYCLES2\n"
+                "%1 = CYCLES\n"
+                "%2 = CYCLES2\n"
+                "CC = %2 == %0\n"
+                "if ! CC jump 1b\n"
+                : "=d,a" (cycle2), "=d,a" (cycle), "=d,a" (usec) : : "CC"
+        );
+
+        usec64 = ((u64)cycle2 << 32) + cycle;
+        do_div(usec64, get_cclk() / USEC_PER_SEC);
+        usec = usec64;
+        if (usec == 0)
+                usec = 1;
+
+        pr_info("PM: resume of kernel completes after  %ld msec %03ld usec\n",
+                usec / USEC_PER_MSEC, usec % USEC_PER_MSEC);
+}
+#endif
+
 static const struct platform_suspend_ops bfin_pm_ops = {
         .enter = bfin_pm_enter,
         .valid  = bfin_pm_valid,
+#ifdef CONFIG_BFIN_PM_WAKEUP_TIME_BENCH
+        .end = bfin_pm_end,
+#endif
 };
 
 static int __init bfin_pm_init(void)