734 lines · c
1// SPDX-License-Identifier: GPL-2.02// Copyright (c) 2011-2017, The Linux Foundation. All rights reserved.3// Copyright (c) 2018, Linaro Limited4 5#include <linux/kernel.h>6#include <linux/module.h>7#include <linux/device.h>8#include <linux/spinlock.h>9#include <linux/idr.h>10#include <linux/slab.h>11#include <linux/workqueue.h>12#include <linux/of_device.h>13#include <linux/soc/qcom/apr.h>14#include <linux/soc/qcom/pdr.h>15#include <linux/rpmsg.h>16#include <linux/of.h>17 18enum {19 PR_TYPE_APR = 0,20 PR_TYPE_GPR,21};22 23/* Some random values tbh which does not collide with static modules */24#define GPR_DYNAMIC_PORT_START 0x1000000025#define GPR_DYNAMIC_PORT_END 0x2000000026 27struct packet_router {28 struct rpmsg_endpoint *ch;29 struct device *dev;30 spinlock_t svcs_lock;31 spinlock_t rx_lock;32 struct idr svcs_idr;33 int dest_domain_id;34 int type;35 struct pdr_handle *pdr;36 struct workqueue_struct *rxwq;37 struct work_struct rx_work;38 struct list_head rx_list;39};40 41struct apr_rx_buf {42 struct list_head node;43 int len;44 uint8_t buf[] __counted_by(len);45};46 47/**48 * apr_send_pkt() - Send a apr message from apr device49 *50 * @adev: Pointer to previously registered apr device.51 * @pkt: Pointer to apr packet to send52 *53 * Return: Will be an negative on packet size on success.54 */55int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt)56{57 struct packet_router *apr = dev_get_drvdata(adev->dev.parent);58 struct apr_hdr *hdr;59 unsigned long flags;60 int ret;61 62 spin_lock_irqsave(&adev->svc.lock, flags);63 64 hdr = &pkt->hdr;65 hdr->src_domain = APR_DOMAIN_APPS;66 hdr->src_svc = adev->svc.id;67 hdr->dest_domain = adev->domain_id;68 hdr->dest_svc = adev->svc.id;69 70 ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);71 spin_unlock_irqrestore(&adev->svc.lock, flags);72 73 return ret ? ret : hdr->pkt_size;74}75EXPORT_SYMBOL_GPL(apr_send_pkt);76 77void gpr_free_port(gpr_port_t *port)78{79 struct packet_router *gpr = port->pr;80 unsigned long flags;81 82 spin_lock_irqsave(&gpr->svcs_lock, flags);83 idr_remove(&gpr->svcs_idr, port->id);84 spin_unlock_irqrestore(&gpr->svcs_lock, flags);85 86 kfree(port);87}88EXPORT_SYMBOL_GPL(gpr_free_port);89 90gpr_port_t *gpr_alloc_port(struct apr_device *gdev, struct device *dev,91 gpr_port_cb cb, void *priv)92{93 struct packet_router *pr = dev_get_drvdata(gdev->dev.parent);94 gpr_port_t *port;95 struct pkt_router_svc *svc;96 int id;97 98 port = kzalloc(sizeof(*port), GFP_KERNEL);99 if (!port)100 return ERR_PTR(-ENOMEM);101 102 svc = port;103 svc->callback = cb;104 svc->pr = pr;105 svc->priv = priv;106 svc->dev = dev;107 spin_lock_init(&svc->lock);108 109 spin_lock(&pr->svcs_lock);110 id = idr_alloc_cyclic(&pr->svcs_idr, svc, GPR_DYNAMIC_PORT_START,111 GPR_DYNAMIC_PORT_END, GFP_ATOMIC);112 if (id < 0) {113 dev_err(dev, "Unable to allocate dynamic GPR src port\n");114 kfree(port);115 spin_unlock(&pr->svcs_lock);116 return ERR_PTR(id);117 }118 119 svc->id = id;120 spin_unlock(&pr->svcs_lock);121 122 return port;123}124EXPORT_SYMBOL_GPL(gpr_alloc_port);125 126static int pkt_router_send_svc_pkt(struct pkt_router_svc *svc, struct gpr_pkt *pkt)127{128 struct packet_router *pr = svc->pr;129 struct gpr_hdr *hdr;130 unsigned long flags;131 int ret;132 133 hdr = &pkt->hdr;134 135 spin_lock_irqsave(&svc->lock, flags);136 ret = rpmsg_trysend(pr->ch, pkt, hdr->pkt_size);137 spin_unlock_irqrestore(&svc->lock, flags);138 139 return ret ? ret : hdr->pkt_size;140}141 142int gpr_send_pkt(struct apr_device *gdev, struct gpr_pkt *pkt)143{144 return pkt_router_send_svc_pkt(&gdev->svc, pkt);145}146EXPORT_SYMBOL_GPL(gpr_send_pkt);147 148int gpr_send_port_pkt(gpr_port_t *port, struct gpr_pkt *pkt)149{150 return pkt_router_send_svc_pkt(port, pkt);151}152EXPORT_SYMBOL_GPL(gpr_send_port_pkt);153 154static void apr_dev_release(struct device *dev)155{156 struct apr_device *adev = to_apr_device(dev);157 158 kfree(adev);159}160 161static int apr_callback(struct rpmsg_device *rpdev, void *buf,162 int len, void *priv, u32 addr)163{164 struct packet_router *apr = dev_get_drvdata(&rpdev->dev);165 struct apr_rx_buf *abuf;166 unsigned long flags;167 168 if (len <= APR_HDR_SIZE) {169 dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n",170 buf, len);171 return -EINVAL;172 }173 174 abuf = kzalloc(struct_size(abuf, buf, len), GFP_ATOMIC);175 if (!abuf)176 return -ENOMEM;177 178 abuf->len = len;179 memcpy(abuf->buf, buf, len);180 181 spin_lock_irqsave(&apr->rx_lock, flags);182 list_add_tail(&abuf->node, &apr->rx_list);183 spin_unlock_irqrestore(&apr->rx_lock, flags);184 185 queue_work(apr->rxwq, &apr->rx_work);186 187 return 0;188}189 190static int apr_do_rx_callback(struct packet_router *apr, struct apr_rx_buf *abuf)191{192 uint16_t hdr_size, msg_type, ver, svc_id;193 struct pkt_router_svc *svc;194 struct apr_device *adev;195 struct apr_driver *adrv = NULL;196 struct apr_resp_pkt resp;197 struct apr_hdr *hdr;198 unsigned long flags;199 void *buf = abuf->buf;200 int len = abuf->len;201 202 hdr = buf;203 ver = APR_HDR_FIELD_VER(hdr->hdr_field);204 if (ver > APR_PKT_VER + 1)205 return -EINVAL;206 207 hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field);208 if (hdr_size < APR_HDR_SIZE) {209 dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size);210 return -EINVAL;211 }212 213 if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {214 dev_err(apr->dev, "APR: Wrong packet size\n");215 return -EINVAL;216 }217 218 msg_type = APR_HDR_FIELD_MT(hdr->hdr_field);219 if (msg_type >= APR_MSG_TYPE_MAX) {220 dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type);221 return -EINVAL;222 }223 224 if (hdr->src_domain >= APR_DOMAIN_MAX ||225 hdr->dest_domain >= APR_DOMAIN_MAX ||226 hdr->src_svc >= APR_SVC_MAX ||227 hdr->dest_svc >= APR_SVC_MAX) {228 dev_err(apr->dev, "APR: Wrong APR header\n");229 return -EINVAL;230 }231 232 svc_id = hdr->dest_svc;233 spin_lock_irqsave(&apr->svcs_lock, flags);234 svc = idr_find(&apr->svcs_idr, svc_id);235 if (svc && svc->dev->driver) {236 adev = svc_to_apr_device(svc);237 adrv = to_apr_driver(adev->dev.driver);238 }239 spin_unlock_irqrestore(&apr->svcs_lock, flags);240 241 if (!adrv || !adev) {242 dev_err(apr->dev, "APR: service is not registered (%d)\n",243 svc_id);244 return -EINVAL;245 }246 247 resp.hdr = *hdr;248 resp.payload_size = hdr->pkt_size - hdr_size;249 250 /*251 * NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include252 * optional headers in to apr_hdr which should be ignored253 */254 if (resp.payload_size > 0)255 resp.payload = buf + hdr_size;256 257 adrv->callback(adev, &resp);258 259 return 0;260}261 262static int gpr_do_rx_callback(struct packet_router *gpr, struct apr_rx_buf *abuf)263{264 uint16_t hdr_size, ver;265 struct pkt_router_svc *svc = NULL;266 struct gpr_resp_pkt resp;267 struct gpr_hdr *hdr;268 unsigned long flags;269 void *buf = abuf->buf;270 int len = abuf->len;271 272 hdr = buf;273 ver = hdr->version;274 if (ver > GPR_PKT_VER + 1)275 return -EINVAL;276 277 hdr_size = hdr->hdr_size;278 if (hdr_size < GPR_PKT_HEADER_WORD_SIZE) {279 dev_err(gpr->dev, "GPR: Wrong hdr size:%d\n", hdr_size);280 return -EINVAL;281 }282 283 if (hdr->pkt_size < GPR_PKT_HEADER_BYTE_SIZE || hdr->pkt_size != len) {284 dev_err(gpr->dev, "GPR: Wrong packet size\n");285 return -EINVAL;286 }287 288 resp.hdr = *hdr;289 resp.payload_size = hdr->pkt_size - (hdr_size * 4);290 291 /*292 * NOTE: hdr_size is not same as GPR_HDR_SIZE as remote can include293 * optional headers in to gpr_hdr which should be ignored294 */295 if (resp.payload_size > 0)296 resp.payload = buf + (hdr_size * 4);297 298 299 spin_lock_irqsave(&gpr->svcs_lock, flags);300 svc = idr_find(&gpr->svcs_idr, hdr->dest_port);301 spin_unlock_irqrestore(&gpr->svcs_lock, flags);302 303 if (!svc) {304 dev_err(gpr->dev, "GPR: Port(%x) is not registered\n",305 hdr->dest_port);306 return -EINVAL;307 }308 309 if (svc->callback)310 svc->callback(&resp, svc->priv, 0);311 312 return 0;313}314 315static void apr_rxwq(struct work_struct *work)316{317 struct packet_router *apr = container_of(work, struct packet_router, rx_work);318 struct apr_rx_buf *abuf, *b;319 unsigned long flags;320 321 if (!list_empty(&apr->rx_list)) {322 list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {323 switch (apr->type) {324 case PR_TYPE_APR:325 apr_do_rx_callback(apr, abuf);326 break;327 case PR_TYPE_GPR:328 gpr_do_rx_callback(apr, abuf);329 break;330 default:331 break;332 }333 spin_lock_irqsave(&apr->rx_lock, flags);334 list_del(&abuf->node);335 spin_unlock_irqrestore(&apr->rx_lock, flags);336 kfree(abuf);337 }338 }339}340 341static int apr_device_match(struct device *dev, const struct device_driver *drv)342{343 struct apr_device *adev = to_apr_device(dev);344 const struct apr_driver *adrv = to_apr_driver(drv);345 const struct apr_device_id *id = adrv->id_table;346 347 /* Attempt an OF style match first */348 if (of_driver_match_device(dev, drv))349 return 1;350 351 if (!id)352 return 0;353 354 while (id->domain_id != 0 || id->svc_id != 0) {355 if (id->domain_id == adev->domain_id &&356 id->svc_id == adev->svc.id)357 return 1;358 id++;359 }360 361 return 0;362}363 364static int apr_device_probe(struct device *dev)365{366 struct apr_device *adev = to_apr_device(dev);367 struct apr_driver *adrv = to_apr_driver(dev->driver);368 int ret;369 370 ret = adrv->probe(adev);371 if (!ret)372 adev->svc.callback = adrv->gpr_callback;373 374 return ret;375}376 377static void apr_device_remove(struct device *dev)378{379 struct apr_device *adev = to_apr_device(dev);380 struct apr_driver *adrv = to_apr_driver(dev->driver);381 struct packet_router *apr = dev_get_drvdata(adev->dev.parent);382 383 if (adrv->remove)384 adrv->remove(adev);385 spin_lock(&apr->svcs_lock);386 idr_remove(&apr->svcs_idr, adev->svc.id);387 spin_unlock(&apr->svcs_lock);388}389 390static int apr_uevent(const struct device *dev, struct kobj_uevent_env *env)391{392 const struct apr_device *adev = to_apr_device(dev);393 int ret;394 395 ret = of_device_uevent_modalias(dev, env);396 if (ret != -ENODEV)397 return ret;398 399 return add_uevent_var(env, "MODALIAS=apr:%s", adev->name);400}401 402const struct bus_type aprbus = {403 .name = "aprbus",404 .match = apr_device_match,405 .probe = apr_device_probe,406 .uevent = apr_uevent,407 .remove = apr_device_remove,408};409EXPORT_SYMBOL_GPL(aprbus);410 411static int apr_add_device(struct device *dev, struct device_node *np,412 u32 svc_id, u32 domain_id)413{414 struct packet_router *apr = dev_get_drvdata(dev);415 struct apr_device *adev = NULL;416 struct pkt_router_svc *svc;417 int ret;418 419 adev = kzalloc(sizeof(*adev), GFP_KERNEL);420 if (!adev)421 return -ENOMEM;422 423 adev->svc_id = svc_id;424 svc = &adev->svc;425 426 svc->id = svc_id;427 svc->pr = apr;428 svc->priv = adev;429 svc->dev = dev;430 spin_lock_init(&svc->lock);431 432 adev->domain_id = domain_id;433 434 if (np)435 snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);436 437 switch (apr->type) {438 case PR_TYPE_APR:439 dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,440 domain_id, svc_id);441 break;442 case PR_TYPE_GPR:443 dev_set_name(&adev->dev, "gprsvc:%s:%x:%x", adev->name,444 domain_id, svc_id);445 break;446 default:447 break;448 }449 450 adev->dev.bus = &aprbus;451 adev->dev.parent = dev;452 adev->dev.of_node = np;453 adev->dev.release = apr_dev_release;454 adev->dev.driver = NULL;455 456 spin_lock(&apr->svcs_lock);457 ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC);458 spin_unlock(&apr->svcs_lock);459 if (ret < 0) {460 dev_err(dev, "idr_alloc failed: %d\n", ret);461 goto out;462 }463 464 /* Protection domain is optional, it does not exist on older platforms */465 ret = of_property_read_string_index(np, "qcom,protection-domain",466 1, &adev->service_path);467 if (ret < 0 && ret != -EINVAL) {468 dev_err(dev, "Failed to read second value of qcom,protection-domain\n");469 goto out;470 }471 472 dev_info(dev, "Adding APR/GPR dev: %s\n", dev_name(&adev->dev));473 474 ret = device_register(&adev->dev);475 if (ret) {476 dev_err(dev, "device_register failed: %d\n", ret);477 put_device(&adev->dev);478 }479 480out:481 return ret;482}483 484static int of_apr_add_pd_lookups(struct device *dev)485{486 const char *service_name, *service_path;487 struct packet_router *apr = dev_get_drvdata(dev);488 struct pdr_service *pds;489 int ret;490 491 for_each_child_of_node_scoped(dev->of_node, node) {492 ret = of_property_read_string_index(node, "qcom,protection-domain",493 0, &service_name);494 if (ret < 0)495 continue;496 497 ret = of_property_read_string_index(node, "qcom,protection-domain",498 1, &service_path);499 if (ret < 0) {500 dev_err(dev, "pdr service path missing: %d\n", ret);501 return ret;502 }503 504 pds = pdr_add_lookup(apr->pdr, service_name, service_path);505 if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {506 dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));507 return PTR_ERR(pds);508 }509 }510 511 return 0;512}513 514static void of_register_apr_devices(struct device *dev, const char *svc_path)515{516 struct packet_router *apr = dev_get_drvdata(dev);517 struct device_node *node;518 const char *service_path;519 int ret;520 521 for_each_child_of_node(dev->of_node, node) {522 u32 svc_id;523 u32 domain_id;524 525 /*526 * This function is called with svc_path NULL during527 * apr_probe(), in which case we register any apr devices528 * without a qcom,protection-domain specified.529 *530 * Then as the protection domains becomes available531 * (if applicable) this function is again called, but with532 * svc_path representing the service becoming available. In533 * this case we register any apr devices with a matching534 * qcom,protection-domain.535 */536 537 ret = of_property_read_string_index(node, "qcom,protection-domain",538 1, &service_path);539 if (svc_path) {540 /* skip APR services that are PD independent */541 if (ret)542 continue;543 544 /* skip APR services whose PD paths don't match */545 if (strcmp(service_path, svc_path))546 continue;547 } else {548 /* skip APR services whose PD lookups are registered */549 if (ret == 0)550 continue;551 }552 553 if (of_property_read_u32(node, "reg", &svc_id))554 continue;555 556 domain_id = apr->dest_domain_id;557 558 if (apr_add_device(dev, node, svc_id, domain_id))559 dev_err(dev, "Failed to add apr %d svc\n", svc_id);560 }561}562 563static int apr_remove_device(struct device *dev, void *svc_path)564{565 struct apr_device *adev = to_apr_device(dev);566 567 if (svc_path && adev->service_path) {568 if (!strcmp(adev->service_path, (char *)svc_path))569 device_unregister(&adev->dev);570 } else {571 device_unregister(&adev->dev);572 }573 574 return 0;575}576 577static void apr_pd_status(int state, char *svc_path, void *priv)578{579 struct packet_router *apr = (struct packet_router *)priv;580 581 switch (state) {582 case SERVREG_SERVICE_STATE_UP:583 of_register_apr_devices(apr->dev, svc_path);584 break;585 case SERVREG_SERVICE_STATE_DOWN:586 device_for_each_child(apr->dev, svc_path, apr_remove_device);587 break;588 }589}590 591static int apr_probe(struct rpmsg_device *rpdev)592{593 struct device *dev = &rpdev->dev;594 struct packet_router *apr;595 int ret;596 597 apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);598 if (!apr)599 return -ENOMEM;600 601 ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id);602 603 if (of_device_is_compatible(dev->of_node, "qcom,gpr")) {604 apr->type = PR_TYPE_GPR;605 } else {606 if (ret) /* try deprecated apr-domain property */607 ret = of_property_read_u32(dev->of_node, "qcom,apr-domain",608 &apr->dest_domain_id);609 apr->type = PR_TYPE_APR;610 }611 612 if (ret) {613 dev_err(dev, "Domain ID not specified in DT\n");614 return ret;615 }616 617 dev_set_drvdata(dev, apr);618 apr->ch = rpdev->ept;619 apr->dev = dev;620 apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");621 if (!apr->rxwq) {622 dev_err(apr->dev, "Failed to start Rx WQ\n");623 return -ENOMEM;624 }625 INIT_WORK(&apr->rx_work, apr_rxwq);626 627 apr->pdr = pdr_handle_alloc(apr_pd_status, apr);628 if (IS_ERR(apr->pdr)) {629 dev_err(dev, "Failed to init PDR handle\n");630 ret = PTR_ERR(apr->pdr);631 goto destroy_wq;632 }633 634 INIT_LIST_HEAD(&apr->rx_list);635 spin_lock_init(&apr->rx_lock);636 spin_lock_init(&apr->svcs_lock);637 idr_init(&apr->svcs_idr);638 639 ret = of_apr_add_pd_lookups(dev);640 if (ret)641 goto handle_release;642 643 of_register_apr_devices(dev, NULL);644 645 return 0;646 647handle_release:648 pdr_handle_release(apr->pdr);649destroy_wq:650 destroy_workqueue(apr->rxwq);651 return ret;652}653 654static void apr_remove(struct rpmsg_device *rpdev)655{656 struct packet_router *apr = dev_get_drvdata(&rpdev->dev);657 658 pdr_handle_release(apr->pdr);659 device_for_each_child(&rpdev->dev, NULL, apr_remove_device);660 destroy_workqueue(apr->rxwq);661}662 663/*664 * __apr_driver_register() - Client driver registration with aprbus665 *666 * @drv:Client driver to be associated with client-device.667 * @owner: owning module/driver668 *669 * This API will register the client driver with the aprbus670 * It is called from the driver's module-init function.671 */672int __apr_driver_register(struct apr_driver *drv, struct module *owner)673{674 drv->driver.bus = &aprbus;675 drv->driver.owner = owner;676 677 return driver_register(&drv->driver);678}679EXPORT_SYMBOL_GPL(__apr_driver_register);680 681/*682 * apr_driver_unregister() - Undo effect of apr_driver_register683 *684 * @drv: Client driver to be unregistered685 */686void apr_driver_unregister(struct apr_driver *drv)687{688 driver_unregister(&drv->driver);689}690EXPORT_SYMBOL_GPL(apr_driver_unregister);691 692static const struct of_device_id pkt_router_of_match[] = {693 { .compatible = "qcom,apr"},694 { .compatible = "qcom,apr-v2"},695 { .compatible = "qcom,gpr"},696 {}697};698MODULE_DEVICE_TABLE(of, pkt_router_of_match);699 700static struct rpmsg_driver packet_router_driver = {701 .probe = apr_probe,702 .remove = apr_remove,703 .callback = apr_callback,704 .drv = {705 .name = "qcom,apr",706 .of_match_table = pkt_router_of_match,707 },708};709 710static int __init apr_init(void)711{712 int ret;713 714 ret = bus_register(&aprbus);715 if (!ret)716 ret = register_rpmsg_driver(&packet_router_driver);717 else718 bus_unregister(&aprbus);719 720 return ret;721}722 723static void __exit apr_exit(void)724{725 bus_unregister(&aprbus);726 unregister_rpmsg_driver(&packet_router_driver);727}728 729subsys_initcall(apr_init);730module_exit(apr_exit);731 732MODULE_LICENSE("GPL v2");733MODULE_DESCRIPTION("Qualcomm APR Bus");734