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