Messages in this thread Patch in this message | | | From | Cristian Marussi <> | Subject | [RFC PATCH 11/16] firmware: arm_scmi: Add test driver generic notification helpers | Date | Sun, 27 Feb 2022 20:56:03 +0000 |
| |
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
| |