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
get_host_mapping(unsigned long auxcore)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 */
load_elf_image_m_core_phdr(unsigned long addr)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
arch_auxiliary_core_up(u32 core_id,ulong addr)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
arch_auxiliary_core_check_up(u32 core_id)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 */
do_bootaux(struct cmd_tbl * cmdtp,int flag,int argc,char * const argv[])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 );
200