[people/ms/u-boot.git] / arch / arm / mach-keystone / cmd_mon.c

/*
 * K2HK: secure kernel command file
 *
 * (C) Copyright 2012-2014
 *     Texas Instruments Incorporated, <www.ti.com>
 *
 * SPDX-License-Identifier:     GPL-2.0+
 */

#include <common.h>
#include <command.h>
asm(".arch_extension sec\n\t");

static int mon_install(u32 addr, u32 dpsc, u32 freq)
{
	int result;

	__asm__ __volatile__ (
		"stmfd r13!, {lr}\n"
		"mov r0, %1\n"
		"mov r1, %2\n"
		"mov r2, %3\n"
		"blx r0\n"
		"ldmfd r13!, {lr}\n"
		: "=&r" (result)
		: "r" (addr), "r" (dpsc), "r" (freq)
		: "cc", "r0", "r1", "r2", "memory");
	return result;
}

static int do_mon_install(cmd_tbl_t *cmdtp, int flag, int argc,
			  char * const argv[])
{
	u32 addr, dpsc_base = 0x1E80000, freq;
	int     rcode = 0;

	if (argc < 2)
		return CMD_RET_USAGE;

	freq = CONFIG_SYS_HZ_CLOCK;

	addr = simple_strtoul(argv[1], NULL, 16);

	rcode = mon_install(addr, dpsc_base, freq);
	printf("## installed monitor, freq [%d], status %d\n",
	       freq, rcode);

	return 0;
}

U_BOOT_CMD(mon_install, 2, 0, do_mon_install,
	   "Install boot kernel at 'addr'",
	   ""
);

static void core_spin(void)
{
	while (1) {
		asm volatile (
			"dsb\n"
			"isb\n"
			"wfi\n"
		);
	}
}

int mon_power_on(int core_id, void *ep)
{
	int result;

	asm volatile (
		"stmfd  r13!, {lr}\n"
		"mov r1, %1\n"
		"mov r2, %2\n"
		"mov r0, #0\n"
		"smc	#0\n"
		"ldmfd  r13!, {lr}\n"
		: "=&r" (result)
		: "r" (core_id), "r" (ep)
		: "cc", "r0", "r1", "r2", "memory");
	return  result;
}

int mon_power_off(int core_id)
{
	int result;

	asm volatile (
		"stmfd  r13!, {lr}\n"
		"mov r1, %1\n"
		"mov r0, #1\n"
		"smc	#1\n"
		"ldmfd  r13!, {lr}\n"
		: "=&r" (result)
		: "r" (core_id)
		: "cc", "r0", "r1", "memory");
	return  result;
}

int do_mon_power(cmd_tbl_t *cmdtp, int flag, int argc,
			char * const argv[])
{
	int     rcode = 0, core_id, on;
	void (*fn)(void);

	fn = core_spin;

	if (argc < 3)
		return CMD_RET_USAGE;

	core_id = simple_strtoul(argv[1], NULL, 16);
	on = simple_strtoul(argv[2], NULL, 16);

	if (on)
		rcode = mon_power_on(core_id, fn);
	else
		rcode = mon_power_off(core_id);

	if (on) {
		if (!rcode)
			printf("core %d powered on successfully\n", core_id);
		else
			printf("core %d power on failure\n", core_id);
	} else {
		printf("core %d powered off successfully\n", core_id);
	}

	return 0;
}

U_BOOT_CMD(mon_power, 3, 0, do_mon_power,
	   "Power On/Off secondary core",
	   "mon_power <coreid> <oper>\n"
	   "- coreid (1-3) and oper (1 - ON, 0 - OFF)\n"
	   ""
);
Commit	Line	Data
ef509b90 VA	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
e6d71e1c	40	freq = CONFIG_SYS_HZ_CLOCK;
ef509b90 VA	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	{
17c5bda2 VA	58	while (1) {
	59	asm volatile (
	60	"dsb\n"
	61	"isb\n"
	62	"wfi\n"
	63	);
	64	}
ef509b90 VA	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	);