arch/arm/mach-imx/imx_bootaux.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2016 Freescale Semiconductor, Inc.
   4  */
   5
   6 #include <log.h>
   7 #include <asm/arch/imx-regs.h>
   8 #include <asm/io.h>
   9 #include <asm/mach-imx/sys_proto.h>
  10 #include <command.h>
  11 #include <elf.h>
  12 #include <imx_sip.h>
  13 #include <vsprintf.h>
  14 #include <linux/arm-smccc.h>
  15 #include <linux/compiler.h>
  16 #include <linux/errno.h>
  17 #include <linux/string.h>
  18 #include <cpu_func.h>
  19
  20 #ifndef CONFIG_IMX8
  21 /* Just to avoid build error */
  22 #if IS_ENABLED(CONFIG_IMX8M)
  23 #define SRC_M4C_NON_SCLR_RST_MASK       BIT(0)
  24 #define SRC_M4_ENABLE_MASK              BIT(0)
  25 #define SRC_M4_REG_OFFSET               0
  26 #endif
  27
  28 __weak const struct rproc_att *imx_bootaux_get_hostmap(void)
  29 {
  30         return NULL;
  31 }
  32
  33 static const struct rproc_att *get_host_mapping(unsigned long auxcore)
  34 {
  35         const struct rproc_att *mmap = imx_bootaux_get_hostmap();
  36
  37         while (mmap && mmap->size) {
  38                 if (mmap->da <= auxcore &&
  39                     mmap->da + mmap->size > auxcore)
  40                         return mmap;
  41                 mmap++;
  42         }
  43
  44         return NULL;
  45 }
  46
  47 /*
  48  * A very simple elf loader for the auxilary core, assumes the image
  49  * is valid, returns the entry point address.
  50  * Translates load addresses in the elf file to the U-Boot address space.
  51  */
  52 static u32 load_elf_image_m_core_phdr(unsigned long addr, u32 *stack)
  53 {
  54         Elf32_Ehdr *ehdr; /* ELF header structure pointer */
  55         Elf32_Phdr *phdr; /* Program header structure pointer */
  56         int num = 0;
  57         int i;
  58
  59         ehdr = (Elf32_Ehdr *)addr;
  60         phdr = (Elf32_Phdr *)(addr + ehdr->e_phoff);
  61
  62         /* Load each program header */
  63         for (i = 0; i < ehdr->e_phnum; ++i, ++phdr) {
  64                 const struct rproc_att *mmap = get_host_mapping(phdr->p_paddr);
  65                 void *dst, *src;
  66
  67                 if (phdr->p_type != PT_LOAD)
  68                         continue;
  69
  70                 if (!mmap) {
  71                         printf("Invalid aux core address: %08x\n",
  72                                phdr->p_paddr);
  73                         return 0;
  74                 }
  75
  76                 dst = (void *)(ulong)(phdr->p_paddr - mmap->da) + mmap->sa;
  77                 src = (void *)addr + phdr->p_offset;
  78
  79                 debug("Loading phdr %i to 0x%p (%i bytes)\n",
  80                       i, dst, phdr->p_filesz);
  81
  82                 if (phdr->p_filesz) {
  83                         memcpy(dst, src, phdr->p_filesz);
  84                         /* Stack in __isr_vector is the first section/word */
  85                         if (!num)
  86                                 *stack = *(uint32_t *)src;
  87                         num++;
  88                 }
  89                 if (phdr->p_filesz != phdr->p_memsz)
  90                         memset(dst + phdr->p_filesz, 0x00,
  91                                phdr->p_memsz - phdr->p_filesz);
  92                 flush_cache((unsigned long)dst &
  93                             ~(CONFIG_SYS_CACHELINE_SIZE - 1),
  94                             ALIGN(phdr->p_filesz, CONFIG_SYS_CACHELINE_SIZE));
  95         }
  96
  97         return ehdr->e_entry;
  98 }
  99
 100 int arch_auxiliary_core_up(u32 core_id, ulong addr)
 101 {
 102         u32 stack, pc;
 103
 104         if (!addr)
 105                 return -EINVAL;
 106
 107         /*
 108          * handling ELF64 binaries
 109          * isn't supported yet.
 110          */
 111         if (valid_elf_image(addr)) {
 112                 pc = load_elf_image_m_core_phdr(addr, &stack);
 113                 if (!pc)
 114                         return CMD_RET_FAILURE;
 115
 116                 if (!IS_ENABLED(CONFIG_ARM64))
 117                         stack = 0x0;
 118         } else {
 119                 /*
 120                  * Assume binary file with vector table at the beginning.
 121                  * Cortex-M4 vector tables start with the stack pointer (SP)
 122                  * and reset vector (initial PC).
 123                  */
 124                 stack = *(u32 *)addr;
 125                 pc = *(u32 *)(addr + 4);
 126         }
 127
 128         printf("## Starting auxiliary core stack = 0x%08X, pc = 0x%08X...\n",
 129                stack, pc);
 130
 131         /* Set the stack and pc to MCU bootROM */
 132         writel(stack, MCU_BOOTROM_BASE_ADDR);
 133         writel(pc, MCU_BOOTROM_BASE_ADDR + 4);
 134
 135         flush_dcache_all();
 136
 137         /* Enable MCU */
 138         if (IS_ENABLED(CONFIG_IMX8M)) {
 139                 arm_smccc_smc(IMX_SIP_SRC, IMX_SIP_SRC_MCU_START, 0, 0, 0, 0, 0, 0, NULL);
 140         } else {
 141                 clrsetbits_le32(SRC_BASE_ADDR + SRC_M4_REG_OFFSET,
 142                                 SRC_M4C_NON_SCLR_RST_MASK, SRC_M4_ENABLE_MASK);
 143         }
 144
 145         return 0;
 146 }
 147
 148 int arch_auxiliary_core_check_up(u32 core_id)
 149 {
 150         struct arm_smccc_res res;
 151         unsigned int val;
 152
 153         if (IS_ENABLED(CONFIG_IMX8M)) {
 154                 arm_smccc_smc(IMX_SIP_SRC, IMX_SIP_SRC_MCU_STARTED, 0, 0, 0, 0, 0, 0, &res);
 155                 return res.a0;
 156         }
 157
 158         val = readl(SRC_BASE_ADDR + SRC_M4_REG_OFFSET);
 159
 160         if (val & SRC_M4C_NON_SCLR_RST_MASK)
 161                 return 0;  /* assert in reset */
 162
 163         return 1;
 164 }
 165 #endif
 166 /*
 167  * To i.MX6SX and i.MX7D, the image supported by bootaux needs
 168  * the reset vector at the head for the image, with SP and PC
 169  * as the first two words.
 170  *
 171  * Per the cortex-M reference manual, the reset vector of M4/M7 needs
 172  * to exist at 0x0 (TCMUL/IDTCM). The PC and SP are the first two addresses
 173  * of that vector.  So to boot M4/M7, the A core must build the M4/M7's reset
 174  * vector with getting the PC and SP from image and filling them to
 175  * TCMUL/IDTCM. When M4/M7 is kicked, it will load the PC and SP by itself.
 176  * The TCMUL/IDTCM is mapped to (MCU_BOOTROM_BASE_ADDR) at A core side for
 177  * accessing the M4/M7 TCMUL/IDTCM.
 178  */
 179 static int do_bootaux(struct cmd_tbl *cmdtp, int flag, int argc,
 180                       char *const argv[])
 181 {
 182         ulong addr;
 183         int ret, up;
 184         u32 core = 0;
 185
 186         if (argc < 2)
 187                 return CMD_RET_USAGE;
 188
 189         if (argc > 2)
 190                 core = simple_strtoul(argv[2], NULL, 10);
 191
 192         up = arch_auxiliary_core_check_up(core);
 193         if (up) {
 194                 printf("## Auxiliary core is already up\n");
 195                 return CMD_RET_SUCCESS;
 196         }
 197
 198         addr = hextoul(argv[1], NULL);
 199
 200         if (!addr)
 201                 return CMD_RET_FAILURE;
 202
 203         ret = arch_auxiliary_core_up(core, addr);
 204         if (ret)
 205                 return CMD_RET_FAILURE;
 206
 207         return CMD_RET_SUCCESS;
 208 }
 209
 210 U_BOOT_CMD(
 211         bootaux, CONFIG_SYS_MAXARGS, 1, do_bootaux,
 212         "Start auxiliary core",
 213         "<address> [<core>]\n"
 214         "   - start auxiliary core [<core>] (default 0),\n"
 215         "     at address <address>\n"
 216 );