boot-monitor: fix power on and power off sequence

Accordingly to PSCI API document the PSCI_CPU_ON just return ALREADY_ON
code for already running CPU. Otherwise it powers it on.

For POWER_OFF another CPU, the CPU just turns the DPSC off for required
CPU, but not wait for completion. If the CPU power off itself and it is not
the CPU 0. It calls WFI instruction, which has to complete the operation.

Signed-off-by: Vitaly Andrianov <vitalya@ti.com>

diff --git a/sec/skern.c b/sec/skern.c
index 91efd79cfcb982f86b9aba5b9e6de82b1917cab4..58550bb38a81387f189bbb62130f8ec0306ba0af 100644 (file)
--- a/sec/skern.c
+++ b/sec/skern.c
@@ -165,6 +165,7 @@ int skern_command(int cmd, const struct regs *regs)
 
        switch (cmd) {
        case 0:
+       case PSCI_CPU_ON:
                error =  skern_poweron_cpu(regs->r1,            /* cpu    */
                                           (void *)regs->r2);   /* entry  */
                break;
@@ -172,10 +173,6 @@ int skern_command(int cmd, const struct regs *regs)
                error =  skern_poweroff_cpu(regs->r1);          /* cpu    */
                break;
 
-       case PSCI_CPU_OFF:
-               error =  skern_poweroff_cpu(chip_get_arm_num());
-               skern_puts("OOPS, PSCI_CPU_OFF DIDN'T WORK\r\n");
-               break;
 #ifdef DEBUG
        case 2:
                mpu_reg = MPU_CFG_REG3;
@@ -196,6 +193,17 @@ int skern_command(int cmd, const struct regs *regs)
                mpu_dump[44]  = 0xdeadbeef;
                break;
 #endif
+       case PSCI_VERSIONS:
+               return 0x00000002;
+
+       case PSCI_SUSPEND:
+               return PSCI_NOT_SUPPORTED;
+
+       case PSCI_CPU_OFF:
+               error =  skern_poweroff_cpu(chip_get_arm_num());
+               skern_puts("OOPS, PSCI_CPU_OFF DIDN'T WORK\r\n");
+               break;
+
        default:
                error = PSCI_NOT_SUPPORTED;
        }
@@ -362,14 +370,8 @@ int skern_poweron_cpu(int cpu_id, void *kern_ep)
        /* power on the DPSC */
        addr += skern_get_dpsc_offset(cpu_id);
        val = *(addr + TETRIS_PD_CPU_N_PDSTAT);
-       if (((val & PD_ACTUAL_STATE_MASK) >> PD_ACTUAL_STATE_SHIFT) == PD_ON) {
-               /* Get the PSM state in the last 5 bits of ret value */
-               val &= MACHINE_PSM_STATE_MASK;
-               val >>= PD_PSM_STATE_SHIFT;
-               /* make it negative */
-               val |= (BIT(31));
-               return (int)val;
-       }
+       if (((val & PD_ACTUAL_STATE_MASK) >> PD_ACTUAL_STATE_SHIFT) == PD_ON)
+               return PSCI_ALREADY_ON;
 
        *(addr + TETRIS_PD_CPU_N_PTCTL) = PD_ON;
        *(addr + TETRIS_PD_CPU_N_PTCMD) = GO_CPU;
@@ -378,8 +380,8 @@ int skern_poweron_cpu(int cpu_id, void *kern_ep)
        while (((*(addr + TETRIS_PD_CPU_N_PDSTAT) >> PD_ACTUAL_STATE_SHIFT)
               & PD_OFF))
                ;
-       return 0;
 
+       return PSCI_SUCCESS;
 }
 
 int skern_poweroff_cpu(int cpu_id)
@@ -406,29 +408,45 @@ int skern_poweroff_cpu(int cpu_id)
 
        /* If we are already off, just return */
        if (((val & PD_ACTUAL_STATE_MASK) >> PD_ACTUAL_STATE_SHIFT) == PD_OFF)
-               return -EINVAL;
+               return PSCI_INV_PARAM;
 
        *(addr + TETRIS_PD_CPU_N_PTCTL) = PD_OFF;
        *(addr + TETRIS_PD_CPU_N_PTCMD) = GO_CPU;
 
        if (cpu_id != chip_get_arm_num()) {
-               /* poll for status change */
-               while (((*(addr + TETRIS_PD_CPU_N_PDSTAT) >> PD_ACTUAL_STATE_SHIFT)
-                       & PD_OFF) != PD_OFF);
-       } else {
-               disable_cache();
-               flush_cache();
-
-               asm volatile (
-               /* disable SMP bit */
-                       "mrc    p15, 0, r8, c1, c0, 1\n"
-                       "bic    r8, r8, #(1 << 6)\n"
-                       "mcr    p15, 0, r8, c1, c0, 1\n"
-                       "isb\n"
-                       "dsb\n"
-                       "wfi\n"
-                       :::"cc", "r8", "memory");
+               /*
+                * CPU A is powering off CPU B
+                * To complete the operation CPU B has to be either in
+                * OFF wait for interrupt/event state orr call wfi/wfe
+                * instruction. CPU cannot just wait here for completion.
+                * So, we just exit from here
+                */
+               return PSCI_SUCCESS;
+       }
+
+       /* CPU wants to kill itself */
+
+       disable_cache();
+       flush_cache();
+
+       asm volatile (
+       /* disable SMP bit */
+               "mrc    p15, 0, r8, c1, c0, 1\n"
+               "bic    r8, r8, #(1 << 6)\n"
+               "mcr    p15, 0, r8, c1, c0, 1\n"
+               "isb\n"
+               "dsb\n"
+               :::"cc", "r8", "memory");
+
+       /*
+        * for all CPUs except CPU0 park here.
+        * CPU 0 is a special case. Before calling WFI it has to disable
+        * tetris clocks and power off tetris power domain
+        */
+       if (cpu_id != 0) {
+               asm volatile ("wfi\n");
+               return PSCI_INTERNAL_FAILURE;
        }
 
-       return 0;
+       return PSCI_SUCCESS;
 }