From ace9429bb58fd418f0c81d4c2835699bddf6bde6 Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Thu, 11 Apr 2024 10:27:49 +0200 Subject: Adding upstream version 6.6.15. Signed-off-by: Daniel Baumann --- drivers/soc/qcom/apr.c | 736 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 736 insertions(+) create mode 100644 drivers/soc/qcom/apr.c (limited to 'drivers/soc/qcom/apr.c') diff --git a/drivers/soc/qcom/apr.c b/drivers/soc/qcom/apr.c new file mode 100644 index 0000000000..30f81d6d9d --- /dev/null +++ b/drivers/soc/qcom/apr.c @@ -0,0 +1,736 @@ +// SPDX-License-Identifier: GPL-2.0 +// Copyright (c) 2011-2017, The Linux Foundation. All rights reserved. +// Copyright (c) 2018, Linaro Limited + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +enum { + PR_TYPE_APR = 0, + PR_TYPE_GPR, +}; + +/* Some random values tbh which does not collide with static modules */ +#define GPR_DYNAMIC_PORT_START 0x10000000 +#define GPR_DYNAMIC_PORT_END 0x20000000 + +struct packet_router { + struct rpmsg_endpoint *ch; + struct device *dev; + spinlock_t svcs_lock; + spinlock_t rx_lock; + struct idr svcs_idr; + int dest_domain_id; + int type; + struct pdr_handle *pdr; + struct workqueue_struct *rxwq; + struct work_struct rx_work; + struct list_head rx_list; +}; + +struct apr_rx_buf { + struct list_head node; + int len; + uint8_t buf[]; +}; + +/** + * apr_send_pkt() - Send a apr message from apr device + * + * @adev: Pointer to previously registered apr device. + * @pkt: Pointer to apr packet to send + * + * Return: Will be an negative on packet size on success. + */ +int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt) +{ + struct packet_router *apr = dev_get_drvdata(adev->dev.parent); + struct apr_hdr *hdr; + unsigned long flags; + int ret; + + spin_lock_irqsave(&adev->svc.lock, flags); + + hdr = &pkt->hdr; + hdr->src_domain = APR_DOMAIN_APPS; + hdr->src_svc = adev->svc.id; + hdr->dest_domain = adev->domain_id; + hdr->dest_svc = adev->svc.id; + + ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size); + spin_unlock_irqrestore(&adev->svc.lock, flags); + + return ret ? ret : hdr->pkt_size; +} +EXPORT_SYMBOL_GPL(apr_send_pkt); + +void gpr_free_port(gpr_port_t *port) +{ + struct packet_router *gpr = port->pr; + unsigned long flags; + + spin_lock_irqsave(&gpr->svcs_lock, flags); + idr_remove(&gpr->svcs_idr, port->id); + spin_unlock_irqrestore(&gpr->svcs_lock, flags); + + kfree(port); +} +EXPORT_SYMBOL_GPL(gpr_free_port); + +gpr_port_t *gpr_alloc_port(struct apr_device *gdev, struct device *dev, + gpr_port_cb cb, void *priv) +{ + struct packet_router *pr = dev_get_drvdata(gdev->dev.parent); + gpr_port_t *port; + struct pkt_router_svc *svc; + int id; + + port = kzalloc(sizeof(*port), GFP_KERNEL); + if (!port) + return ERR_PTR(-ENOMEM); + + svc = port; + svc->callback = cb; + svc->pr = pr; + svc->priv = priv; + svc->dev = dev; + spin_lock_init(&svc->lock); + + spin_lock(&pr->svcs_lock); + id = idr_alloc_cyclic(&pr->svcs_idr, svc, GPR_DYNAMIC_PORT_START, + GPR_DYNAMIC_PORT_END, GFP_ATOMIC); + if (id < 0) { + dev_err(dev, "Unable to allocate dynamic GPR src port\n"); + kfree(port); + spin_unlock(&pr->svcs_lock); + return ERR_PTR(id); + } + + svc->id = id; + spin_unlock(&pr->svcs_lock); + + return port; +} +EXPORT_SYMBOL_GPL(gpr_alloc_port); + +static int pkt_router_send_svc_pkt(struct pkt_router_svc *svc, struct gpr_pkt *pkt) +{ + struct packet_router *pr = svc->pr; + struct gpr_hdr *hdr; + unsigned long flags; + int ret; + + hdr = &pkt->hdr; + + spin_lock_irqsave(&svc->lock, flags); + ret = rpmsg_trysend(pr->ch, pkt, hdr->pkt_size); + spin_unlock_irqrestore(&svc->lock, flags); + + return ret ? ret : hdr->pkt_size; +} + +int gpr_send_pkt(struct apr_device *gdev, struct gpr_pkt *pkt) +{ + return pkt_router_send_svc_pkt(&gdev->svc, pkt); +} +EXPORT_SYMBOL_GPL(gpr_send_pkt); + +int gpr_send_port_pkt(gpr_port_t *port, struct gpr_pkt *pkt) +{ + return pkt_router_send_svc_pkt(port, pkt); +} +EXPORT_SYMBOL_GPL(gpr_send_port_pkt); + +static void apr_dev_release(struct device *dev) +{ + struct apr_device *adev = to_apr_device(dev); + + kfree(adev); +} + +static int apr_callback(struct rpmsg_device *rpdev, void *buf, + int len, void *priv, u32 addr) +{ + struct packet_router *apr = dev_get_drvdata(&rpdev->dev); + struct apr_rx_buf *abuf; + unsigned long flags; + + if (len <= APR_HDR_SIZE) { + dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n", + buf, len); + return -EINVAL; + } + + abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC); + if (!abuf) + return -ENOMEM; + + abuf->len = len; + memcpy(abuf->buf, buf, len); + + spin_lock_irqsave(&apr->rx_lock, flags); + list_add_tail(&abuf->node, &apr->rx_list); + spin_unlock_irqrestore(&apr->rx_lock, flags); + + queue_work(apr->rxwq, &apr->rx_work); + + return 0; +} + +static int apr_do_rx_callback(struct packet_router *apr, struct apr_rx_buf *abuf) +{ + uint16_t hdr_size, msg_type, ver, svc_id; + struct pkt_router_svc *svc; + struct apr_device *adev; + struct apr_driver *adrv = NULL; + struct apr_resp_pkt resp; + struct apr_hdr *hdr; + unsigned long flags; + void *buf = abuf->buf; + int len = abuf->len; + + hdr = buf; + ver = APR_HDR_FIELD_VER(hdr->hdr_field); + if (ver > APR_PKT_VER + 1) + return -EINVAL; + + hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field); + if (hdr_size < APR_HDR_SIZE) { + dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size); + return -EINVAL; + } + + if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) { + dev_err(apr->dev, "APR: Wrong packet size\n"); + return -EINVAL; + } + + msg_type = APR_HDR_FIELD_MT(hdr->hdr_field); + if (msg_type >= APR_MSG_TYPE_MAX) { + dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type); + return -EINVAL; + } + + if (hdr->src_domain >= APR_DOMAIN_MAX || + hdr->dest_domain >= APR_DOMAIN_MAX || + hdr->src_svc >= APR_SVC_MAX || + hdr->dest_svc >= APR_SVC_MAX) { + dev_err(apr->dev, "APR: Wrong APR header\n"); + return -EINVAL; + } + + svc_id = hdr->dest_svc; + spin_lock_irqsave(&apr->svcs_lock, flags); + svc = idr_find(&apr->svcs_idr, svc_id); + if (svc && svc->dev->driver) { + adev = svc_to_apr_device(svc); + adrv = to_apr_driver(adev->dev.driver); + } + spin_unlock_irqrestore(&apr->svcs_lock, flags); + + if (!adrv || !adev) { + dev_err(apr->dev, "APR: service is not registered (%d)\n", + svc_id); + return -EINVAL; + } + + resp.hdr = *hdr; + resp.payload_size = hdr->pkt_size - hdr_size; + + /* + * NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include + * optional headers in to apr_hdr which should be ignored + */ + if (resp.payload_size > 0) + resp.payload = buf + hdr_size; + + adrv->callback(adev, &resp); + + return 0; +} + +static int gpr_do_rx_callback(struct packet_router *gpr, struct apr_rx_buf *abuf) +{ + uint16_t hdr_size, ver; + struct pkt_router_svc *svc = NULL; + struct gpr_resp_pkt resp; + struct gpr_hdr *hdr; + unsigned long flags; + void *buf = abuf->buf; + int len = abuf->len; + + hdr = buf; + ver = hdr->version; + if (ver > GPR_PKT_VER + 1) + return -EINVAL; + + hdr_size = hdr->hdr_size; + if (hdr_size < GPR_PKT_HEADER_WORD_SIZE) { + dev_err(gpr->dev, "GPR: Wrong hdr size:%d\n", hdr_size); + return -EINVAL; + } + + if (hdr->pkt_size < GPR_PKT_HEADER_BYTE_SIZE || hdr->pkt_size != len) { + dev_err(gpr->dev, "GPR: Wrong packet size\n"); + return -EINVAL; + } + + resp.hdr = *hdr; + resp.payload_size = hdr->pkt_size - (hdr_size * 4); + + /* + * NOTE: hdr_size is not same as GPR_HDR_SIZE as remote can include + * optional headers in to gpr_hdr which should be ignored + */ + if (resp.payload_size > 0) + resp.payload = buf + (hdr_size * 4); + + + spin_lock_irqsave(&gpr->svcs_lock, flags); + svc = idr_find(&gpr->svcs_idr, hdr->dest_port); + spin_unlock_irqrestore(&gpr->svcs_lock, flags); + + if (!svc) { + dev_err(gpr->dev, "GPR: Port(%x) is not registered\n", + hdr->dest_port); + return -EINVAL; + } + + if (svc->callback) + svc->callback(&resp, svc->priv, 0); + + return 0; +} + +static void apr_rxwq(struct work_struct *work) +{ + struct packet_router *apr = container_of(work, struct packet_router, rx_work); + struct apr_rx_buf *abuf, *b; + unsigned long flags; + + if (!list_empty(&apr->rx_list)) { + list_for_each_entry_safe(abuf, b, &apr->rx_list, node) { + switch (apr->type) { + case PR_TYPE_APR: + apr_do_rx_callback(apr, abuf); + break; + case PR_TYPE_GPR: + gpr_do_rx_callback(apr, abuf); + break; + default: + break; + } + spin_lock_irqsave(&apr->rx_lock, flags); + list_del(&abuf->node); + spin_unlock_irqrestore(&apr->rx_lock, flags); + kfree(abuf); + } + } +} + +static int apr_device_match(struct device *dev, struct device_driver *drv) +{ + struct apr_device *adev = to_apr_device(dev); + struct apr_driver *adrv = to_apr_driver(drv); + const struct apr_device_id *id = adrv->id_table; + + /* Attempt an OF style match first */ + if (of_driver_match_device(dev, drv)) + return 1; + + if (!id) + return 0; + + while (id->domain_id != 0 || id->svc_id != 0) { + if (id->domain_id == adev->domain_id && + id->svc_id == adev->svc.id) + return 1; + id++; + } + + return 0; +} + +static int apr_device_probe(struct device *dev) +{ + struct apr_device *adev = to_apr_device(dev); + struct apr_driver *adrv = to_apr_driver(dev->driver); + int ret; + + ret = adrv->probe(adev); + if (!ret) + adev->svc.callback = adrv->gpr_callback; + + return ret; +} + +static void apr_device_remove(struct device *dev) +{ + struct apr_device *adev = to_apr_device(dev); + struct apr_driver *adrv = to_apr_driver(dev->driver); + struct packet_router *apr = dev_get_drvdata(adev->dev.parent); + + if (adrv->remove) + adrv->remove(adev); + spin_lock(&apr->svcs_lock); + idr_remove(&apr->svcs_idr, adev->svc.id); + spin_unlock(&apr->svcs_lock); +} + +static int apr_uevent(const struct device *dev, struct kobj_uevent_env *env) +{ + const struct apr_device *adev = to_apr_device(dev); + int ret; + + ret = of_device_uevent_modalias(dev, env); + if (ret != -ENODEV) + return ret; + + return add_uevent_var(env, "MODALIAS=apr:%s", adev->name); +} + +struct bus_type aprbus = { + .name = "aprbus", + .match = apr_device_match, + .probe = apr_device_probe, + .uevent = apr_uevent, + .remove = apr_device_remove, +}; +EXPORT_SYMBOL_GPL(aprbus); + +static int apr_add_device(struct device *dev, struct device_node *np, + u32 svc_id, u32 domain_id) +{ + struct packet_router *apr = dev_get_drvdata(dev); + struct apr_device *adev = NULL; + struct pkt_router_svc *svc; + int ret; + + adev = kzalloc(sizeof(*adev), GFP_KERNEL); + if (!adev) + return -ENOMEM; + + adev->svc_id = svc_id; + svc = &adev->svc; + + svc->id = svc_id; + svc->pr = apr; + svc->priv = adev; + svc->dev = dev; + spin_lock_init(&svc->lock); + + adev->domain_id = domain_id; + + if (np) + snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np); + + switch (apr->type) { + case PR_TYPE_APR: + dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name, + domain_id, svc_id); + break; + case PR_TYPE_GPR: + dev_set_name(&adev->dev, "gprsvc:%s:%x:%x", adev->name, + domain_id, svc_id); + break; + default: + break; + } + + adev->dev.bus = &aprbus; + adev->dev.parent = dev; + adev->dev.of_node = np; + adev->dev.release = apr_dev_release; + adev->dev.driver = NULL; + + spin_lock(&apr->svcs_lock); + ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC); + spin_unlock(&apr->svcs_lock); + if (ret < 0) { + dev_err(dev, "idr_alloc failed: %d\n", ret); + goto out; + } + + /* Protection domain is optional, it does not exist on older platforms */ + ret = of_property_read_string_index(np, "qcom,protection-domain", + 1, &adev->service_path); + if (ret < 0 && ret != -EINVAL) { + dev_err(dev, "Failed to read second value of qcom,protection-domain\n"); + goto out; + } + + dev_info(dev, "Adding APR/GPR dev: %s\n", dev_name(&adev->dev)); + + ret = device_register(&adev->dev); + if (ret) { + dev_err(dev, "device_register failed: %d\n", ret); + put_device(&adev->dev); + } + +out: + return ret; +} + +static int of_apr_add_pd_lookups(struct device *dev) +{ + const char *service_name, *service_path; + struct packet_router *apr = dev_get_drvdata(dev); + struct device_node *node; + struct pdr_service *pds; + int ret; + + for_each_child_of_node(dev->of_node, node) { + ret = of_property_read_string_index(node, "qcom,protection-domain", + 0, &service_name); + if (ret < 0) + continue; + + ret = of_property_read_string_index(node, "qcom,protection-domain", + 1, &service_path); + if (ret < 0) { + dev_err(dev, "pdr service path missing: %d\n", ret); + of_node_put(node); + return ret; + } + + pds = pdr_add_lookup(apr->pdr, service_name, service_path); + if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) { + dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds)); + of_node_put(node); + return PTR_ERR(pds); + } + } + + return 0; +} + +static void of_register_apr_devices(struct device *dev, const char *svc_path) +{ + struct packet_router *apr = dev_get_drvdata(dev); + struct device_node *node; + const char *service_path; + int ret; + + for_each_child_of_node(dev->of_node, node) { + u32 svc_id; + u32 domain_id; + + /* + * This function is called with svc_path NULL during + * apr_probe(), in which case we register any apr devices + * without a qcom,protection-domain specified. + * + * Then as the protection domains becomes available + * (if applicable) this function is again called, but with + * svc_path representing the service becoming available. In + * this case we register any apr devices with a matching + * qcom,protection-domain. + */ + + ret = of_property_read_string_index(node, "qcom,protection-domain", + 1, &service_path); + if (svc_path) { + /* skip APR services that are PD independent */ + if (ret) + continue; + + /* skip APR services whose PD paths don't match */ + if (strcmp(service_path, svc_path)) + continue; + } else { + /* skip APR services whose PD lookups are registered */ + if (ret == 0) + continue; + } + + if (of_property_read_u32(node, "reg", &svc_id)) + continue; + + domain_id = apr->dest_domain_id; + + if (apr_add_device(dev, node, svc_id, domain_id)) + dev_err(dev, "Failed to add apr %d svc\n", svc_id); + } +} + +static int apr_remove_device(struct device *dev, void *svc_path) +{ + struct apr_device *adev = to_apr_device(dev); + + if (svc_path && adev->service_path) { + if (!strcmp(adev->service_path, (char *)svc_path)) + device_unregister(&adev->dev); + } else { + device_unregister(&adev->dev); + } + + return 0; +} + +static void apr_pd_status(int state, char *svc_path, void *priv) +{ + struct packet_router *apr = (struct packet_router *)priv; + + switch (state) { + case SERVREG_SERVICE_STATE_UP: + of_register_apr_devices(apr->dev, svc_path); + break; + case SERVREG_SERVICE_STATE_DOWN: + device_for_each_child(apr->dev, svc_path, apr_remove_device); + break; + } +} + +static int apr_probe(struct rpmsg_device *rpdev) +{ + struct device *dev = &rpdev->dev; + struct packet_router *apr; + int ret; + + apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL); + if (!apr) + return -ENOMEM; + + ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id); + + if (of_device_is_compatible(dev->of_node, "qcom,gpr")) { + apr->type = PR_TYPE_GPR; + } else { + if (ret) /* try deprecated apr-domain property */ + ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", + &apr->dest_domain_id); + apr->type = PR_TYPE_APR; + } + + if (ret) { + dev_err(dev, "Domain ID not specified in DT\n"); + return ret; + } + + dev_set_drvdata(dev, apr); + apr->ch = rpdev->ept; + apr->dev = dev; + apr->rxwq = create_singlethread_workqueue("qcom_apr_rx"); + if (!apr->rxwq) { + dev_err(apr->dev, "Failed to start Rx WQ\n"); + return -ENOMEM; + } + INIT_WORK(&apr->rx_work, apr_rxwq); + + apr->pdr = pdr_handle_alloc(apr_pd_status, apr); + if (IS_ERR(apr->pdr)) { + dev_err(dev, "Failed to init PDR handle\n"); + ret = PTR_ERR(apr->pdr); + goto destroy_wq; + } + + INIT_LIST_HEAD(&apr->rx_list); + spin_lock_init(&apr->rx_lock); + spin_lock_init(&apr->svcs_lock); + idr_init(&apr->svcs_idr); + + ret = of_apr_add_pd_lookups(dev); + if (ret) + goto handle_release; + + of_register_apr_devices(dev, NULL); + + return 0; + +handle_release: + pdr_handle_release(apr->pdr); +destroy_wq: + destroy_workqueue(apr->rxwq); + return ret; +} + +static void apr_remove(struct rpmsg_device *rpdev) +{ + struct packet_router *apr = dev_get_drvdata(&rpdev->dev); + + pdr_handle_release(apr->pdr); + device_for_each_child(&rpdev->dev, NULL, apr_remove_device); + destroy_workqueue(apr->rxwq); +} + +/* + * __apr_driver_register() - Client driver registration with aprbus + * + * @drv:Client driver to be associated with client-device. + * @owner: owning module/driver + * + * This API will register the client driver with the aprbus + * It is called from the driver's module-init function. + */ +int __apr_driver_register(struct apr_driver *drv, struct module *owner) +{ + drv->driver.bus = &aprbus; + drv->driver.owner = owner; + + return driver_register(&drv->driver); +} +EXPORT_SYMBOL_GPL(__apr_driver_register); + +/* + * apr_driver_unregister() - Undo effect of apr_driver_register + * + * @drv: Client driver to be unregistered + */ +void apr_driver_unregister(struct apr_driver *drv) +{ + driver_unregister(&drv->driver); +} +EXPORT_SYMBOL_GPL(apr_driver_unregister); + +static const struct of_device_id pkt_router_of_match[] = { + { .compatible = "qcom,apr"}, + { .compatible = "qcom,apr-v2"}, + { .compatible = "qcom,gpr"}, + {} +}; +MODULE_DEVICE_TABLE(of, pkt_router_of_match); + +static struct rpmsg_driver packet_router_driver = { + .probe = apr_probe, + .remove = apr_remove, + .callback = apr_callback, + .drv = { + .name = "qcom,apr", + .of_match_table = pkt_router_of_match, + }, +}; + +static int __init apr_init(void) +{ + int ret; + + ret = bus_register(&aprbus); + if (!ret) + ret = register_rpmsg_driver(&packet_router_driver); + else + bus_unregister(&aprbus); + + return ret; +} + +static void __exit apr_exit(void) +{ + bus_unregister(&aprbus); + unregister_rpmsg_driver(&packet_router_driver); +} + +subsys_initcall(apr_init); +module_exit(apr_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("Qualcomm APR Bus"); -- cgit v1.2.3