[PATCH v2 6/9] remoteproc: mediatek: Add SCP core 1 SRAM offset

[Tinghan Shen <tinghan.shen@xxxxxxxxxxxx>]

Because SCP core 0 and core 1 both boot from address 0 and have the same
viewpoint of memory, HW has a set of registers, "SRAM offset", to add
offset to accessed address for SCP core 1 to solve this problem.

The "SRAM offset" configuration is composed by specifying a range and an
offset. The value of range is from the viewpoint of SCP core 1.
When SCP core 1 accessing addresses in the configured range, SCP bus
adds an offset to shift the destination on SCP SRAM. This shift is
transparent to the software running on SCP core 1.

Signed-off-by: Tinghan Shen <tinghan.shen@xxxxxxxxxxxx>
---
 drivers/remoteproc/mtk_scp.c | 42 ++++++++++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

diff --git a/drivers/remoteproc/mtk_scp.c b/drivers/remoteproc/mtk_scp.c
index 731a8094c373..b8a4db581179 100644
--- a/drivers/remoteproc/mtk_scp.c
+++ b/drivers/remoteproc/mtk_scp.c
@@ -505,6 +505,27 @@ static int mt8195_scp_before_load(struct mtk_scp *scp)
 static int mt8195_scp_dual_before_load(struct mtk_scp *scp)
 {
 	u32 sec_ctrl;
+	struct device *dev = scp->dev;
+	struct device_node *main_np;
+	struct platform_device *main_pdev;
+	struct mtk_scp *scp_core0;
+
+	/* Get sram start address from SCP core 0 */
+	main_np = of_parse_phandle(dev->of_node, "mediatek,scp-core", 0);
+	if (!main_np) {
+		dev_warn(dev, "Invalid SCP main core phandle\n");
+		return -EINVAL;
+	}
+
+	main_pdev = of_find_device_by_node(main_np);
+	of_node_put(main_np);
+
+	if (!main_pdev) {
+		dev_err(dev, "Cannot find SCP core 0 device\n");
+		return -ENODEV;
+	}
+	scp_core0 = platform_get_drvdata(main_pdev);
+	put_device(&main_pdev->dev);
 
 	scp_sram_power_on(scp->reg_base + MT8195_CPU1_SRAM_PD, 0);
 
@@ -514,6 +535,27 @@ static int mt8195_scp_dual_before_load(struct mtk_scp *scp)
 	/* enable MPU for all memory regions */
 	writel(0xff, scp->reg_base + MT8195_CORE1_MEM_ATT_PREDEF);
 
+	/* The value of SRAM offset range is from the viewpoint of SCP core 1.
+	 * This configuration adds an offset on SCP bus when SCP core 1 accesses SCP SRAM
+	 * to solve the SCP core 0 and core 1 both fetch the 1st instruction from the same
+	 * SRAM address.
+	 *
+	 * Because SCP core 0 and core 1 both boot from address 0, this must be configured
+	 * before boot SCP core 1.
+	 *
+	 * Configure the range of SRAM addresses will be added offset.
+	 */
+	writel(0, scp->reg_base + MT8195_L2TCM_OFFSET_RANGE_0_LOW);
+	writel(scp->sram_size, scp->reg_base + MT8195_L2TCM_OFFSET_RANGE_0_HIGH);
+
+	/* configure the offset value */
+	writel(scp->sram_phys - scp_core0->sram_phys, scp->reg_base + MT8195_L2TCM_OFFSET);
+
+	/* enable adding sram offset when fetching instruction and data */
+	sec_ctrl = readl(scp->reg_base + MT8195_SEC_CTRL);
+	sec_ctrl |= MT8195_CORE_OFFSET_ENABLE_I | MT8195_CORE_OFFSET_ENABLE_D;
+	writel(sec_ctrl, scp->reg_base + MT8195_SEC_CTRL);
+
 	return 0;
 }
 
-- 
2.18.0