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