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