[RFC PATCH 11/16] firmware: arm_scmi: Add test driver generic notification helpers

[Cristian Marussi <cristian.marussi@arm.com>]

Add a few generic helpers to allow the test driver to register a common
notifier block and related read routines in order to test notifications
when supported by the specific SCMI protocol.

Signed-off-by: Cristian Marussi <cristian.marussi@arm.com>
---
 .../arm_scmi/scmi_test_driver/test_common.c   | 177 ++++++++++++++++++
 .../arm_scmi/scmi_test_driver/test_common.h   |  28 +++
 2 files changed, 205 insertions(+)

diff --git a/drivers/firmware/arm_scmi/scmi_test_driver/test_common.c b/drivers/firmware/arm_scmi/scmi_test_driver/test_common.c
index cbe2eec4f2ac..c44fe702196e 100644
--- a/drivers/firmware/arm_scmi/scmi_test_driver/test_common.c
+++ b/drivers/firmware/arm_scmi/scmi_test_driver/test_common.c
@@ -14,6 +14,22 @@
 
 #include "test_common.h"
 
+struct scmi_test_setup *
+scmi_test_tsp_shallow_copy(struct device *dev,
+			   struct scmi_test_setup *tsp)
+{
+	struct scmi_test_setup *tsp_copy;
+
+	tsp_copy = devm_kzalloc(dev, sizeof(*tsp_copy), GFP_KERNEL);
+	if (!tsp_copy)
+		return ERR_PTR(-ENOMEM);
+
+	memcpy(tsp_copy, tsp, sizeof(*tsp_copy));
+	tsp_copy->n_priv = NULL;
+
+	return tsp_copy;
+}
+
 /* Common File operations */
 
 /**
@@ -70,6 +86,167 @@ int scmi_test_release(struct inode *ino, struct file *filp)
 	return 0;
 }
 
+int scmi_test_generic_notif_cb(struct notifier_block *nb, unsigned long event,
+			       void *data)
+{
+	struct scmi_test_notif_reports *nrep;
+	void *d_report;
+
+	nrep = container_of(nb, struct scmi_test_notif_reports, notif_nb);
+
+	mutex_lock(&nrep->mtx);
+	d_report = ((u8 *)nrep->reports) +
+		    ((nrep->count % nrep->max_reports) * nrep->report_sz);
+	memcpy(d_report, data, nrep->report_sz);
+	nrep->count++;
+	mutex_unlock(&nrep->mtx);
+
+	return NOTIFY_OK;
+}
+
+static struct scmi_test_notif_reports *
+scmi_test_notif_reports_alloc(struct scmi_device *sdev,
+			      size_t report_sz, unsigned int max_reports,
+			      u8 proto_id, u8 evt_id, const u32 *src_id,
+			      notifier_fn_t notif_cb)
+{
+	int ret;
+	struct scmi_test_notif_reports *nrep;
+	struct device *dev = &sdev->dev;
+	const struct scmi_notify_ops *n_ops = sdev->handle->notify_ops;
+
+	nrep = devm_kzalloc(dev, sizeof(*nrep), GFP_KERNEL);
+	if (!nrep)
+		return ERR_PTR(-ENOMEM);
+
+	nrep->reports = devm_kcalloc(dev, max_reports, report_sz, GFP_KERNEL);
+	if (!nrep->reports) {
+		devm_kfree(dev, nrep);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	nrep->proto_id = proto_id;
+	nrep->evt_id = evt_id;
+	if (src_id)
+		nrep->src_id = *src_id;
+	else
+		nrep->src_id = 0xffffffff;
+	nrep->report_sz = report_sz;
+	nrep->max_reports = max_reports;
+	mutex_init(&nrep->mtx);
+
+	if (notif_cb)
+		nrep->notif_nb.notifier_call = notif_cb;
+	else
+		nrep->notif_nb.notifier_call = scmi_test_generic_notif_cb;
+
+	ret = n_ops->devm_event_notifier_register(sdev,
+						  proto_id, evt_id, src_id,
+						  &nrep->notif_nb);
+	if (ret) {
+		devm_kfree(dev, nrep->reports);
+		devm_kfree(dev, nrep);
+		return ERR_PTR(ret);
+	}
+
+	return nrep;
+}
+
+static int scmi_test_notif_reports_free(struct scmi_device *sdev,
+					struct scmi_test_notif_reports *nrep)
+{
+	int ret;
+	struct device *dev = &sdev->dev;
+	const struct scmi_notify_ops *n_ops = sdev->handle->notify_ops;
+
+	if (!nrep)
+		return -EINVAL;
+
+	ret = n_ops->devm_event_notifier_unregister(sdev, &nrep->notif_nb);
+	if (ret)
+		return ret;
+
+	devm_kfree(dev, nrep->reports);
+	devm_kfree(dev, nrep);
+
+	return 0;
+}
+
+int scmi_test_notif_manage(struct scmi_test_setup *tsp, bool enabled,
+			   u8 proto_id, u8 evt_id, const u32 *src_id,
+			   size_t rep_sz, notifier_fn_t notif_cb)
+{
+	int ret;
+
+	/* For testing allow enable only for one notifier at time */
+	if ((enabled && tsp->n_priv) || (!enabled && !tsp->n_priv))
+		return -EINVAL;
+
+	if (enabled) {
+		tsp->n_priv =
+			scmi_test_notif_reports_alloc(tsp->sdev, rep_sz,
+						      SCMI_TEST_MAX_REPORTS,
+						      proto_id, evt_id,
+						      src_id, notif_cb);
+		if (IS_ERR(tsp->n_priv)) {
+			ret = PTR_ERR(tsp->n_priv);
+			tsp->n_priv = NULL;
+			return ret;
+		}
+	} else {
+		ret = scmi_test_notif_reports_free(tsp->sdev, tsp->n_priv);
+		if (ret)
+			return ret;
+
+		tsp->n_priv = NULL;
+	}
+
+	return 0;
+}
+
+ssize_t scmi_test_notif_reports_read(struct file *filp, char __user *buf,
+				     size_t count, loff_t *ppos)
+{
+	struct scmi_test_buffer *data = filp->private_data;
+	struct scmi_test_setup *tsp = filp->f_inode->i_private;
+	struct scmi_test_notif_reports *nrep = tsp->n_priv;
+
+	if (!data || !nrep)
+		return 0;
+
+	if (!data->used) {
+		int i, cnt;
+
+		mutex_lock(&nrep->mtx);
+		cnt = nrep->count >= nrep->max_reports ?
+			nrep->max_reports : nrep->count;
+		for (i = 0; i < cnt; i++) {
+			u8 *rep;
+			int j;
+
+			data->used += scnprintf(data->buf + data->used,
+						data->len - data->used,
+				       "[%02u/%02u|%u] - |%02X|%u|%u| = Report: ",
+						i,
+						nrep->max_reports, nrep->count,
+						nrep->proto_id, nrep->evt_id,
+						nrep->src_id);
+
+			rep = ((u8 *)nrep->reports) + i * nrep->report_sz;
+			for (j = 0; j < nrep->report_sz; j++)
+				data->used += scnprintf(data->buf + data->used,
+							data->len - data->used,
+							"%02X ", rep[j]);
+
+			data->used += scnprintf(data->buf + data->used,
+						data->len - data->used, "\n");
+		}
+		mutex_unlock(&nrep->mtx);
+	}
+
+	return simple_read_from_buffer(buf, count, ppos, data->buf, data->used);
+}
+
 int scmi_test_fixed_buffer_open(struct inode *ino, struct file *filp)
 {
 	struct scmi_test_buffer *data;
diff --git a/drivers/firmware/arm_scmi/scmi_test_driver/test_common.h b/drivers/firmware/arm_scmi/scmi_test_driver/test_common.h
index e02c2521f090..93c05db0ebff 100644
--- a/drivers/firmware/arm_scmi/scmi_test_driver/test_common.h
+++ b/drivers/firmware/arm_scmi/scmi_test_driver/test_common.h
@@ -66,8 +66,36 @@ struct scmi_test_buffer {
 	unsigned char buf[];
 };
 
+#define SCMI_TEST_MAX_REPORTS	20
+struct scmi_test_notif_reports {
+	u8 proto_id;
+	u8 evt_id;
+	u32 src_id;
+	unsigned int max_reports;
+	size_t report_sz;
+	void *reports;
+	unsigned int count;
+	struct notifier_block notif_nb;
+	/* Protect access to notification reports */
+	struct mutex mtx;
+};
+
 extern const struct file_operations scmi_test_string_file_fops;
 
+int scmi_test_generic_notif_cb(struct notifier_block *nb, unsigned long event,
+			       void *data);
+
+int scmi_test_notif_manage(struct scmi_test_setup *tsp, bool enabled,
+			   u8 proto_id, u8 evt_id, const u32 *src_id,
+			   size_t rep_sz, notifier_fn_t notif_cb);
+
+ssize_t scmi_test_notif_reports_read(struct file *filp, char __user *buf,
+				     size_t count, loff_t *ppos);
+
+struct scmi_test_setup *
+scmi_test_tsp_shallow_copy(struct device *dev,
+			   struct scmi_test_setup *tsp);
+
 int scmi_test_setup_open(struct inode *ino, struct file *filp);
 int scmi_test_fixed_buffer_open(struct inode *ino, struct file *filp);
 int scmi_test_release(struct inode *ino, struct file *filp);
-- 
2.17.1


_______________________________________________
linux-arm-kernel mailing list
linux-arm-kernel@lists.infradead.org
http://lists.infradead.org/mailman/listinfo/linux-arm-kernel