arch/arm/mach-keystone/cmd_mon.c

   1 /*
   2  * K2HK: secure kernel command file
   3  *
   4  * (C) Copyright 2012-2014
   5  *     Texas Instruments Incorporated, <www.ti.com>
   6  *
   7  * SPDX-License-Identifier:     GPL-2.0+
   8  */
   9
  10 #include <common.h>
  11 #include <command.h>
  12 asm(".arch_extension sec\n\t");
  13
  14 static int mon_install(u32 addr, u32 dpsc, u32 freq)
  15 {
  16         int result;
  17
  18         __asm__ __volatile__ (
  19                 "stmfd r13!, {lr}\n"
  20                 "mov r0, %1\n"
  21                 "mov r1, %2\n"
  22                 "mov r2, %3\n"
  23                 "blx r0\n"
  24                 "ldmfd r13!, {lr}\n"
  25                 : "=&r" (result)
  26                 : "r" (addr), "r" (dpsc), "r" (freq)
  27                 : "cc", "r0", "r1", "r2", "memory");
  28         return result;
  29 }
  30
  31 static int do_mon_install(cmd_tbl_t *cmdtp, int flag, int argc,
  32                           char * const argv[])
  33 {
  34         u32 addr, dpsc_base = 0x1E80000, freq;
  35         int     rcode = 0;
  36
  37         if (argc < 2)
  38                 return CMD_RET_USAGE;
  39
  40         freq = clk_get_rate(sys_clk0_6_clk);
  41
  42         addr = simple_strtoul(argv[1], NULL, 16);
  43
  44         rcode = mon_install(addr, dpsc_base, freq);
  45         printf("## installed monitor, freq [%d], status %d\n",
  46                freq, rcode);
  47
  48         return 0;
  49 }
  50
  51 U_BOOT_CMD(mon_install, 2, 0, do_mon_install,
  52            "Install boot kernel at 'addr'",
  53            ""
  54 );
  55
  56 static void core_spin(void)
  57 {
  58         while (1) {
  59                 asm volatile (
  60                         "dsb\n"
  61                         "isb\n"
  62                         "wfi\n"
  63                 );
  64         }
  65 }
  66
  67 int mon_power_on(int core_id, void *ep)
  68 {
  69         int result;
  70
  71         asm volatile (
  72                 "stmfd  r13!, {lr}\n"
  73                 "mov r1, %1\n"
  74                 "mov r2, %2\n"
  75                 "mov r0, #0\n"
  76                 "smc    #0\n"
  77                 "ldmfd  r13!, {lr}\n"
  78                 : "=&r" (result)
  79                 : "r" (core_id), "r" (ep)
  80                 : "cc", "r0", "r1", "r2", "memory");
  81         return  result;
  82 }
  83
  84 int mon_power_off(int core_id)
  85 {
  86         int result;
  87
  88         asm volatile (
  89                 "stmfd  r13!, {lr}\n"
  90                 "mov r1, %1\n"
  91                 "mov r0, #1\n"
  92                 "smc    #1\n"
  93                 "ldmfd  r13!, {lr}\n"
  94                 : "=&r" (result)
  95                 : "r" (core_id)
  96                 : "cc", "r0", "r1", "memory");
  97         return  result;
  98 }
  99
 100 int do_mon_power(cmd_tbl_t *cmdtp, int flag, int argc,
 101                         char * const argv[])
 102 {
 103         int     rcode = 0, core_id, on;
 104         void (*fn)(void);
 105
 106         fn = core_spin;
 107
 108         if (argc < 3)
 109                 return CMD_RET_USAGE;
 110
 111         core_id = simple_strtoul(argv[1], NULL, 16);
 112         on = simple_strtoul(argv[2], NULL, 16);
 113
 114         if (on)
 115                 rcode = mon_power_on(core_id, fn);
 116         else
 117                 rcode = mon_power_off(core_id);
 118
 119         if (on) {
 120                 if (!rcode)
 121                         printf("core %d powered on successfully\n", core_id);
 122                 else
 123                         printf("core %d power on failure\n", core_id);
 124         } else {
 125                 printf("core %d powered off successfully\n", core_id);
 126         }
 127
 128         return 0;
 129 }
 130
 131 U_BOOT_CMD(mon_power, 3, 0, do_mon_power,
 132            "Power On/Off secondary core",
 133            "mon_power <coreid> <oper>\n"
 134            "- coreid (1-3) and oper (1 - ON, 0 - OFF)\n"
 135            ""
 136 );