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