Now that we have basic support for all the protocols in the specification, let's probe them individually and initialise them. Cc: Arnd Bergmann <arnd@xxxxxxxx> Signed-off-by: Sudeep Holla <sudeep.holla@xxxxxxx> --- drivers/firmware/arm_scmi/common.h | 5 +++ drivers/firmware/arm_scmi/driver.c | 82 +++++++++++++++++++++++++++++++++++++- 2 files changed, 86 insertions(+), 1 deletion(-) diff --git a/drivers/firmware/arm_scmi/common.h b/drivers/firmware/arm_scmi/common.h index 0df52905ba48..07acddf5c060 100644 --- a/drivers/firmware/arm_scmi/common.h +++ b/drivers/firmware/arm_scmi/common.h @@ -119,4 +119,9 @@ int scmi_version_get(const struct scmi_handle *h, u8 protocol, u32 *version); void scmi_setup_protocol_implemented(const struct scmi_handle *handle, u8 *prot_imp); +typedef int (*scmi_prot_init_fn_t)(struct scmi_handle *); int scmi_base_protocol_init(struct scmi_handle *h); +int scmi_perf_protocol_init(struct scmi_handle *h); +int scmi_sensors_protocol_init(struct scmi_handle *h); +int scmi_power_protocol_init(struct scmi_handle *h); +int scmi_clock_protocol_init(struct scmi_handle *h); diff --git a/drivers/firmware/arm_scmi/driver.c b/drivers/firmware/arm_scmi/driver.c index 5df76512a291..a1abcf2049ca 100644 --- a/drivers/firmware/arm_scmi/driver.c +++ b/drivers/firmware/arm_scmi/driver.c @@ -154,6 +154,12 @@ struct scmi_shared_mem { u8 msg_payload[0]; }; +struct scmi_protocol_match { + u8 protocol_id; + scmi_prot_init_fn_t fn; + char name[32]; +}; + static int scmi_linux_errmap[] = { /* better than switch case as long as return value is continuous */ 0, /* SCMI_SUCCESS */ @@ -686,6 +692,39 @@ static int scmi_xfer_info_init(struct scmi_info *sinfo) return 0; } +static const struct scmi_protocol_match scmi_protocols[] = { + { + .protocol_id = SCMI_PROTOCOL_PERF, + .fn = scmi_perf_protocol_init, + .name = "scmi-cpufreq", + }, { + .protocol_id = SCMI_PROTOCOL_CLOCK, + .fn = scmi_clock_protocol_init, + .name = "scmi-clocks", + }, { + .protocol_id = SCMI_PROTOCOL_POWER, + .fn = scmi_power_protocol_init, + .name = "scmi-power-domain", + }, { + .protocol_id = SCMI_PROTOCOL_SENSOR, + .fn = scmi_sensors_protocol_init, + .name = "scmi-hwmon", + }, + {} +}; + +static const struct scmi_protocol_match *scmi_protocol_match_get(u8 protocol_id) +{ + int i; + const struct scmi_protocol_match *loop = scmi_protocols; + + for (i = 0; i < ARRAY_SIZE(scmi_protocols); i++, loop++) + if (loop->protocol_id == protocol_id) + return loop; + + return NULL; +} + static int scmi_mailbox_check(struct device_node *np) { struct of_phandle_args arg; @@ -767,6 +806,27 @@ static inline int scmi_mbox_chan_setup(struct scmi_info *info) return 0; } +static inline void +scmi_create_protocol_device(struct device_node *np, struct scmi_info *info, + const struct scmi_protocol_match *match) +{ + int init_ret; + struct platform_device *cdev; + + cdev = of_platform_device_create(np, match->name, info->dev); + if (!cdev) { + dev_err(info->dev, "failed to create %s device\n", match->name); + return; + } + + init_ret = match->fn(&info->handle); + if (init_ret) { + dev_err(info->dev, "SCMI protocol %d init error %d\n", + match->protocol_id, init_ret); + of_platform_device_destroy(&cdev->dev, NULL); + } +} + static int scmi_probe(struct platform_device *pdev) { int ret; @@ -774,7 +834,7 @@ static int scmi_probe(struct platform_device *pdev) const struct scmi_desc *desc; struct scmi_info *info; struct device *dev = &pdev->dev; - struct device_node *np = dev->of_node; + struct device_node *child, *np = dev->of_node; /* Only mailbox method supported, check for the presence of one */ if (scmi_mailbox_check(np)) { @@ -813,6 +873,26 @@ static int scmi_probe(struct platform_device *pdev) return ret; } + for_each_available_child_of_node(np, child) { + u32 prot_id; + const struct scmi_protocol_match *match; + + if (of_property_read_u32(child, "reg", &prot_id)) + continue; + + prot_id &= MSG_PROTOCOL_ID_MASK; + + if (!scmi_is_protocol_implemented(handle, prot_id)) { + dev_err(dev, "SCMI protocol %d not implemented\n", + prot_id); + continue; + } + + match = scmi_protocol_match_get(prot_id); + if (match) + scmi_create_protocol_device(child, info, match); + } + mutex_lock(&scmi_list_mutex); list_add_tail(&info->node, &scmi_list); mutex_unlock(&scmi_list_mutex); -- 2.7.4 -- To unsubscribe from this list: send the line "unsubscribe devicetree" in the body of a message to majordomo@xxxxxxxxxxxxxxx More majordomo info at http://vger.kernel.org/majordomo-info.html