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