diff --git a/arch/arm/mach-keystone/cmd_mon.c b/arch/arm/mach-keystone/cmd_mon.c index 6a9bdc9601..591e75826b 100644 --- a/arch/arm/mach-keystone/cmd_mon.c +++ b/arch/arm/mach-keystone/cmd_mon.c @@ -9,14 +9,16 @@ #include #include +#include #include asm(".arch_extension sec\n\t"); static int do_mon_install(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[]) { - u32 addr, dpsc_base = 0x1E80000, freq; + u32 addr, dpsc_base = 0x1E80000, freq, load_addr, size; int rcode = 0; + struct image_header *header; if (argc < 2) return CMD_RET_USAGE; @@ -25,9 +27,21 @@ static int do_mon_install(cmd_tbl_t *cmdtp, int flag, int argc, addr = simple_strtoul(argv[1], NULL, 16); - rcode = mon_install(addr, dpsc_base, freq); - printf("## installed monitor, freq [%d], status %d\n", - freq, rcode); + header = (struct image_header *)addr; + + if (image_get_magic(header) != IH_MAGIC) { + printf("## Please update monitor image\n"); + return -EFAULT; + } + + load_addr = image_get_load(header); + size = image_get_data_size(header); + memcpy((void *)load_addr, (void *)(addr + sizeof(struct image_header)), + size); + + rcode = mon_install(load_addr, dpsc_base, freq); + printf("## installed monitor @ 0x%x, freq [%d], status %d\n", + load_addr, freq, rcode); return 0; }