[PATCH 2/8] xilinx: zynqmp: merge firmware calls for EL2 and EL3

[Michal Simek]

From: Ibai Erkiaga <ibai.erkiaga-elorza at xilinx.com>

This patch merges ZynqMP firmware calls under xilinx_pm_request in order
to make trainsparent the EL. Calls at EL3 are send through IPI messages
and EL2 through SMC calls.

The EL2 call uses fixed payload and arg size as the EL3 call. The
firmware is capable to handle PMUFW_PAYLOAD_ARG_CNT bytes but the
firmware API is limited by the SMC call size.

Signed-off-by: Ibai Erkiaga <ibai.erkiaga-elorza at xilinx.com>
Signed-off-by: Michal Simek <michal.simek at xilinx.com>
---

 drivers/firmware/firmware-zynqmp.c | 84 +++++++++++++++---------------
 1 file changed, 41 insertions(+), 43 deletions(-)

diff --git a/drivers/firmware/firmware-zynqmp.c b/drivers/firmware/firmware-zynqmp.c
index 66edc1693016..41394d487ad5 100644
--- a/drivers/firmware/firmware-zynqmp.c
+++ b/drivers/firmware/firmware-zynqmp.c
@@ -37,6 +37,7 @@ static int ipi_req(const u32 *req, size_t req_len, u32 *res, size_t res_maxlen)
 	if (!(zynqmp_power.tx_chan.dev) || !(&zynqmp_power.rx_chan.dev))
 		return -EINVAL;
 
+	debug("%s, Sending IPI message with ID: 0x%0x\n", __func__, req[0]);
 	msg.buf = (u32 *)req;
 	msg.len = req_len;
 	ret = mbox_send(&zynqmp_power.tx_chan, &msg);
@@ -54,14 +55,6 @@ static int ipi_req(const u32 *req, size_t req_len, u32 *res, size_t res_maxlen)
 	return ret;
 }
 
-static int send_req(const u32 *req, size_t req_len, u32 *res, size_t res_maxlen)
-{
-	if (IS_ENABLED(CONFIG_SPL_BUILD) || current_el() == 3)
-		return ipi_req(req, req_len, res, res_maxlen);
-
-	return xilinx_pm_request(req[0], 0, 0, 0, 0, res);
-}
-
 unsigned int zynqmp_firmware_version(void)
 {
 	int ret;
@@ -74,9 +67,9 @@ unsigned int zynqmp_firmware_version(void)
 	 * asking PMUFW again.
 	 **/
 	if (pm_api_version == ZYNQMP_PM_VERSION_INVALID) {
-		const u32 request[] = { PM_GET_API_VERSION };
 
-		ret = send_req(request, ARRAY_SIZE(request), ret_payload, 2);
+		ret = xilinx_pm_request(PM_GET_API_VERSION, 0, 0, 0, 0,
+					ret_payload);
 		if (ret)
 			panic("PMUFW is not found - Please load it!\n");
 
@@ -97,16 +90,13 @@ unsigned int zynqmp_firmware_version(void)
  */
 void zynqmp_pmufw_load_config_object(const void *cfg_obj, size_t size)
 {
-	const u32 request[] = {
-		PM_SET_CONFIGURATION,
-		(u32)((u64)cfg_obj)
-	};
-	u32 response = 0;
 	int err;
+	u32 ret_payload[PAYLOAD_ARG_CNT];
 
 	printf("Loading new PMUFW cfg obj (%ld bytes)\n", size);
 
-	err = send_req(request, ARRAY_SIZE(request), &response, 1);
+	err = xilinx_pm_request(PM_SET_CONFIGURATION, (u32)(u64)cfg_obj, 0, 0,
+				0, ret_payload);
 	if (err == XST_PM_NO_ACCESS) {
 		printf("PMUFW no permission to change config object\n");
 		return;
@@ -115,10 +105,10 @@ void zynqmp_pmufw_load_config_object(const void *cfg_obj, size_t size)
 	if (err)
 		printf("Cannot load PMUFW configuration object (%d)\n", err);
 
-	if (response)
-		printf("PMUFW returned 0x%08x status!\n", response);
+	if (ret_payload[0])
+		printf("PMUFW returned 0x%08x status!\n", ret_payload[0]);
 
-	if ((err || response) && IS_ENABLED(CONFIG_SPL_BUILD))
+	if ((err || ret_payload[0]) && IS_ENABLED(CONFIG_SPL_BUILD))
 		panic("PMUFW config object loading failed in EL3\n");
 }
 
@@ -164,32 +154,40 @@ U_BOOT_DRIVER(zynqmp_power) = {
 int __maybe_unused xilinx_pm_request(u32 api_id, u32 arg0, u32 arg1, u32 arg2,
 				     u32 arg3, u32 *ret_payload)
 {
-	/*
-	 * Added SIP service call Function Identifier
-	 * Make sure to stay in x0 register
-	 */
-	struct pt_regs regs;
-
-	if (current_el() == 3) {
-		printf("%s: Can't call SMC from EL3 context\n", __func__);
-		return -EPERM;
-	}
-
-	regs.regs[0] = PM_SIP_SVC | api_id;
-	regs.regs[1] = ((u64)arg1 << 32) | arg0;
-	regs.regs[2] = ((u64)arg3 << 32) | arg2;
+	debug("%s at EL%d, API ID: 0x%0x\n", __func__, current_el(), api_id);
+
+	if (IS_ENABLED(CONFIG_SPL_BUILD) || current_el() == 3) {
+		/*
+		 * Use fixed payload and arg size as the EL2 call. The firmware
+		 * is capable to handle PMUFW_PAYLOAD_ARG_CNT bytes but the
+		 * firmware API is limited by the SMC call size
+		 */
+		u32 regs[] = {api_id, arg0, arg1, arg2, arg3};
+
+		ipi_req(regs, PAYLOAD_ARG_CNT, ret_payload, PAYLOAD_ARG_CNT);
+	} else {
+		/*
+		 * Added SIP service call Function Identifier
+		 * Make sure to stay in x0 register
+		 */
+		struct pt_regs regs;
+
+		regs.regs[0] = PM_SIP_SVC | api_id;
+		regs.regs[1] = ((u64)arg1 << 32) | arg0;
+		regs.regs[2] = ((u64)arg3 << 32) | arg2;
+
+		smc_call(&regs);
+
+		if (ret_payload) {
+			ret_payload[0] = (u32)regs.regs[0];
+			ret_payload[1] = upper_32_bits(regs.regs[0]);
+			ret_payload[2] = (u32)regs.regs[1];
+			ret_payload[3] = upper_32_bits(regs.regs[1]);
+			ret_payload[4] = (u32)regs.regs[2];
+		}
 
-	smc_call(&regs);
-
-	if (ret_payload) {
-		ret_payload[0] = (u32)regs.regs[0];
-		ret_payload[1] = upper_32_bits(regs.regs[0]);
-		ret_payload[2] = (u32)regs.regs[1];
-		ret_payload[3] = upper_32_bits(regs.regs[1]);
-		ret_payload[4] = (u32)regs.regs[2];
 	}
-
-	return regs.regs[0];
+	return (ret_payload) ? ret_payload[0] : 0;
 }
 
 static const struct udevice_id zynqmp_firmware_ids[] = {
-- 
2.27.0