imx_bootaux.c 4.5 KB

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