[PATCH v2 2/2] firmware: arm_scmi: Augment SMC/HVC to allow optional parameters

[Nikunj Kela <quic_nkela@xxxxxxxxxxx>]

This patch add support for passing parameters to smc/hvc calls.
This patch is useful when multiple scmi instances are using same
smc-id and firmware needs to distiguish among the instances.

Signed-off-by: Nikunj Kela <quic_nkela@xxxxxxxxxxx>
---
 drivers/firmware/arm_scmi/smc.c | 72 ++++++++++++++++++++++++++++++++-
 1 file changed, 71 insertions(+), 1 deletion(-)

diff --git a/drivers/firmware/arm_scmi/smc.c b/drivers/firmware/arm_scmi/smc.c
index 93272e4bbd12..c57d2f3fab87 100644
--- a/drivers/firmware/arm_scmi/smc.c
+++ b/drivers/firmware/arm_scmi/smc.c
@@ -20,6 +20,16 @@
 
 #include "common.h"
 
+#define MAX_PARAM_COUNT 6
+
+/**
+ * scmi_smc_param_t - parameter type for SCMI smc/hvc call
+ */
+typedef union {
+	u64 x;
+	u32 w;
+} scmi_smc_param_t;
+
 /**
  * struct scmi_smc - Structure representing a SCMI smc transport
  *
@@ -30,6 +40,8 @@
  * @inflight: Atomic flag to protect access to Tx/Rx shared memory area.
  *	      Used when operating in atomic mode.
  * @func_id: smc/hvc call function id
+ * @is_smc64: A flag, indicating smc64 calling convention.
+ * @params: Optional, smc/hvc call parameters.
  */
 
 struct scmi_smc {
@@ -40,8 +52,55 @@ struct scmi_smc {
 #define INFLIGHT_NONE	MSG_TOKEN_MAX
 	atomic_t inflight;
 	u32 func_id;
+	bool is_smc64;
+	scmi_smc_param_t params[MAX_PARAM_COUNT];
 };
 
+static void populate_smc_params(struct device *dev, struct scmi_smc *scmi_info)
+{
+	struct device_node *np = dev->of_node;
+	u64 params64[MAX_PARAM_COUNT] = { 0 };
+	u32 params32[MAX_PARAM_COUNT] = { 0 };
+	int i, count;
+
+#ifdef CONFIG_ARM64
+	if (scmi_info->is_smc64) {
+		count = of_property_read_variable_u64_array(np,
+							    "arm,smc64-params",
+							    &params64[0], 1,
+							    MAX_PARAM_COUNT);
+		if (count == -EINVAL)  /* if property is not defined */
+			return;
+
+		if (count > 0)	/* populate the parameters */
+			for (i = 0; i < count; i++)
+				scmi_info->params[i].x = params64[i];
+		else
+			goto param_err;
+	} else {
+#else
+	{
+#endif
+		count = of_property_read_variable_u32_array(np,
+							    "arm,smc32-params",
+							    &params32[0], 1,
+							    MAX_PARAM_COUNT);
+		if (count == -EINVAL)  /* if property is not defined */
+			return;
+
+		if (count > 0)	/* populate the parameters */
+			for (i = 0; i < count; i++)
+				scmi_info->params[i].w = params32[i];
+		else
+			goto param_err;
+	}
+
+	return;
+
+param_err:
+	dev_warn(dev, "failed to read smc/hvc call parameters\n");
+}
+
 static irqreturn_t smc_msg_done_isr(int irq, void *data)
 {
 	struct scmi_smc *scmi_info = data;
@@ -156,6 +215,8 @@ static int smc_chan_setup(struct scmi_chan_info *cinfo, struct device *dev,
 	}
 
 	scmi_info->func_id = func_id;
+	scmi_info->is_smc64 = ARM_SMCCC_IS_64(func_id);
+	populate_smc_params(dev, scmi_info);
 	scmi_info->cinfo = cinfo;
 	smc_channel_lock_init(scmi_info);
 	cinfo->transport_info = scmi_info;
@@ -179,6 +240,7 @@ static int smc_send_message(struct scmi_chan_info *cinfo,
 {
 	struct scmi_smc *scmi_info = cinfo->transport_info;
 	struct arm_smccc_res res;
+	scmi_smc_param_t *p = scmi_info->params;
 
 	/*
 	 * Channel will be released only once response has been
@@ -188,7 +250,15 @@ static int smc_send_message(struct scmi_chan_info *cinfo,
 
 	shmem_tx_prepare(scmi_info->shmem, xfer, cinfo);
 
-	arm_smccc_1_1_invoke(scmi_info->func_id, 0, 0, 0, 0, 0, 0, 0, &res);
+#ifdef CONFIG_ARM64
+	if (scmi_info->is_smc64)
+		arm_smccc_1_1_invoke(scmi_info->func_id, p[0].x, p[1].x, p[2].x,
+				     p[3].x, p[4].x, p[5].x, 0, &res);
+	else
+#endif
+		arm_smccc_1_1_invoke(scmi_info->func_id, p[0].w, p[1].w, p[2].w,
+				     p[3].w, p[4].w, p[5].w, 0, &res);
+
 
 	/* Only SMCCC_RET_NOT_SUPPORTED is valid error code */
 	if (res.a0) {
-- 
2.17.1