[PATCH v3 2/3] remoteproc: imx_rproc: Program non-zero SM CPU/LMM reset vector

[Peng Fan (OSS)]

From: Peng Fan <peng.fan@xxxxxxx>

Cortex-M[7,33] processors use a fixed reset vector table format:

  0x00  Initial SP value
  0x04  Reset vector
  0x08  NMI
  0x0C  ...
  ...
  IRQ[n]

In ELF images, the corresponding layout is:

reset_vectors:  --> hardware reset address
        .word __stack_end__
        .word Reset_Handler
        .word NMI_Handler
        .word HardFault_Handler
        ...
        .word UART_IRQHandler
        .word SPI_IRQHandler
        ...

Reset_Handler:  --> ELF entry point address
        ...

The hardware fetches the first two words from reset_vectors and populates
SP with __stack_end__ and PC with Reset_Handler. Execution proceeds from
Reset_Handler.

However, the ELF entry point does not always match the hardware reset
address. For example, on i.MX94 CM33S:

  ELF entry point:     0x0ffc211d
  hardware reset base: 0x0ffc0000 (default reset value, sw programmable)

Current driver always programs the reset vector as 0. But i.MX94 CM33S's
default reset base is 0x0ffc0000, so the correct reset vector must be
passed to the SM API; otherwise the M33 Sync core cannot boot successfully.

rproc_elf_get_boot_addr() returns the ELF entry point, which is not the
hardware reset vector address. Fix the issue by deriving the hardware reset
vector locally using a SoC-specific mask:

  reset_vector = rproc->bootaddr & reset_vector_mask

The ELF entry point semantics remain unchanged. The masking is applied only
at the point where the SM reset vector is programmed.

Add reset_vector_mask = GENMASK_U32(31, 16) to the i.MX95 M7 configuration
so the hardware reset vector is derived correctly. Without this mask, the
SM reset vector would be programmed with an unaligned ELF entry point and
the M7 core would fail to boot.

Signed-off-by: Peng Fan <peng.fan@xxxxxxx>
---
 drivers/remoteproc/imx_rproc.c | 27 +++++++++++++++++++++++++--
 drivers/remoteproc/imx_rproc.h |  2 ++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/drivers/remoteproc/imx_rproc.c b/drivers/remoteproc/imx_rproc.c
index 0dd80e688b0ea3df4c66e5726884dc86c8a5a881..c21782be4bb69d830479f538a091bda48b740ca4 100644
--- a/drivers/remoteproc/imx_rproc.c
+++ b/drivers/remoteproc/imx_rproc.c
@@ -339,13 +339,32 @@ static int imx_rproc_scu_api_start(struct rproc *rproc)
 	return imx_sc_pm_cpu_start(priv->ipc_handle, priv->rsrc_id, true, priv->entry);
 }
 
+static u64 imx_rproc_sm_get_reset_vector(struct rproc *rproc)
+{
+	struct imx_rproc *priv = rproc->priv;
+	u32 reset_vector_mask = priv->dcfg->reset_vector_mask ?: GENMASK(31, 0);
+
+	/*
+	 * The hardware fetches the first two words from reset_vectors
+	 * (hardware reset address) and populates SP and PC using the first
+	 * two words. Execution proceeds from PC. The ELF entry point does
+	 * not always match the hardware reset address.
+	 * To derive the correct hardware reset address, the lower address
+	 * bits must be masked off before programming the reset vector.
+	 */
+	return rproc->bootaddr & reset_vector_mask;
+}
+
 static int imx_rproc_sm_cpu_start(struct rproc *rproc)
 {
 	struct imx_rproc *priv = rproc->priv;
 	const struct imx_rproc_dcfg *dcfg = priv->dcfg;
+	u64 reset_vector;
 	int ret;
 
-	ret = scmi_imx_cpu_reset_vector_set(dcfg->cpuid, 0, true, false, false);
+	reset_vector = imx_rproc_sm_get_reset_vector(rproc);
+
+	ret = scmi_imx_cpu_reset_vector_set(dcfg->cpuid, reset_vector, true, false, false);
 	if (ret) {
 		dev_err(priv->dev, "Failed to set reset vector cpuid(%u): %d\n", dcfg->cpuid, ret);
 		return ret;
@@ -359,13 +378,16 @@ static int imx_rproc_sm_lmm_start(struct rproc *rproc)
 	struct imx_rproc *priv = rproc->priv;
 	const struct imx_rproc_dcfg *dcfg = priv->dcfg;
 	struct device *dev = priv->dev;
+	u64 reset_vector;
 	int ret;
 
+	reset_vector = imx_rproc_sm_get_reset_vector(rproc);
+
 	/*
 	 * If the remoteproc core can't start the M7, it will already be
 	 * handled in imx_rproc_sm_lmm_prepare().
 	 */
-	ret = scmi_imx_lmm_reset_vector_set(dcfg->lmid, dcfg->cpuid, 0, 0);
+	ret = scmi_imx_lmm_reset_vector_set(dcfg->lmid, dcfg->cpuid, 0, reset_vector);
 	if (ret) {
 		dev_err(dev, "Failed to set reset vector lmid(%u), cpuid(%u): %d\n",
 			dcfg->lmid, dcfg->cpuid, ret);
@@ -1462,6 +1484,7 @@ static const struct imx_rproc_dcfg imx_rproc_cfg_imx95_m7 = {
 	/* Must align with System Manager Firmware */
 	.cpuid		= 1, /* Use 1 as cpu id for M7 core */
 	.lmid		= 1, /* Use 1 as Logical Machine ID where M7 resides */
+	.reset_vector_mask = GENMASK_U32(31, 16),
 };
 
 static const struct of_device_id imx_rproc_of_match[] = {
diff --git a/drivers/remoteproc/imx_rproc.h b/drivers/remoteproc/imx_rproc.h
index d37e6f90548cec727b4aeb874680b42af85bdbb4..0d7d48352a1091ad24e8e083172ce6da6d26ae10 100644
--- a/drivers/remoteproc/imx_rproc.h
+++ b/drivers/remoteproc/imx_rproc.h
@@ -41,6 +41,8 @@ struct imx_rproc_dcfg {
 	/* For System Manager(SM) based SoCs */
 	u32				cpuid; /* ID of the remote core */
 	u32				lmid;  /* ID of the Logcial Machine */
+	/* reset_vector = elf_entry_addr & reset_vector_mask */
+	u32				reset_vector_mask;
 };
 
 #endif /* _IMX_RPROC_H */

-- 
2.37.1