1// SPDX-License-Identifier: GPL-2.0 2// Copyright (c) 2011-2017, The Linux Foundation. All rights reserved. 3// Copyright (c) 2018, Linaro Limited 4 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 0x10000000 25#define GPR_DYNAMIC_PORT_END 0x20000000 26 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 device 49 * 50 * @adev: Pointer to previously registered apr device. 51 * @pkt: Pointer to apr packet to send 52 * 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 include 252 * optional headers in to apr_hdr which should be ignored 253 */ 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 include 293 * optional headers in to gpr_hdr which should be ignored 294 */ 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, struct device_driver *drv) 342{ 343 struct apr_device *adev = to_apr_device(dev); 344 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 device_node *node; 489 struct pdr_service *pds; 490 int ret; 491 492 for_each_child_of_node(dev->of_node, node) { 493 ret = of_property_read_string_index(node, "qcom,protection-domain", 494 0, &service_name); 495 if (ret < 0) 496 continue; 497 498 ret = of_property_read_string_index(node, "qcom,protection-domain", 499 1, &service_path); 500 if (ret < 0) { 501 dev_err(dev, "pdr service path missing: %d\n", ret); 502 of_node_put(node); 503 return ret; 504 } 505 506 pds = pdr_add_lookup(apr->pdr, service_name, service_path); 507 if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) { 508 dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds)); 509 of_node_put(node); 510 return PTR_ERR(pds); 511 } 512 } 513 514 return 0; 515} 516 517static void of_register_apr_devices(struct device *dev, const char *svc_path) 518{ 519 struct packet_router *apr = dev_get_drvdata(dev); 520 struct device_node *node; 521 const char *service_path; 522 int ret; 523 524 for_each_child_of_node(dev->of_node, node) { 525 u32 svc_id; 526 u32 domain_id; 527 528 /* 529 * This function is called with svc_path NULL during 530 * apr_probe(), in which case we register any apr devices 531 * without a qcom,protection-domain specified. 532 * 533 * Then as the protection domains becomes available 534 * (if applicable) this function is again called, but with 535 * svc_path representing the service becoming available. In 536 * this case we register any apr devices with a matching 537 * qcom,protection-domain. 538 */ 539 540 ret = of_property_read_string_index(node, "qcom,protection-domain", 541 1, &service_path); 542 if (svc_path) { 543 /* skip APR services that are PD independent */ 544 if (ret) 545 continue; 546 547 /* skip APR services whose PD paths don't match */ 548 if (strcmp(service_path, svc_path)) 549 continue; 550 } else { 551 /* skip APR services whose PD lookups are registered */ 552 if (ret == 0) 553 continue; 554 } 555 556 if (of_property_read_u32(node, "reg", &svc_id)) 557 continue; 558 559 domain_id = apr->dest_domain_id; 560 561 if (apr_add_device(dev, node, svc_id, domain_id)) 562 dev_err(dev, "Failed to add apr %d svc\n", svc_id); 563 } 564} 565 566static int apr_remove_device(struct device *dev, void *svc_path) 567{ 568 struct apr_device *adev = to_apr_device(dev); 569 570 if (svc_path && adev->service_path) { 571 if (!strcmp(adev->service_path, (char *)svc_path)) 572 device_unregister(&adev->dev); 573 } else { 574 device_unregister(&adev->dev); 575 } 576 577 return 0; 578} 579 580static void apr_pd_status(int state, char *svc_path, void *priv) 581{ 582 struct packet_router *apr = (struct packet_router *)priv; 583 584 switch (state) { 585 case SERVREG_SERVICE_STATE_UP: 586 of_register_apr_devices(apr->dev, svc_path); 587 break; 588 case SERVREG_SERVICE_STATE_DOWN: 589 device_for_each_child(apr->dev, svc_path, apr_remove_device); 590 break; 591 } 592} 593 594static int apr_probe(struct rpmsg_device *rpdev) 595{ 596 struct device *dev = &rpdev->dev; 597 struct packet_router *apr; 598 int ret; 599 600 apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL); 601 if (!apr) 602 return -ENOMEM; 603 604 ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id); 605 606 if (of_device_is_compatible(dev->of_node, "qcom,gpr")) { 607 apr->type = PR_TYPE_GPR; 608 } else { 609 if (ret) /* try deprecated apr-domain property */ 610 ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", 611 &apr->dest_domain_id); 612 apr->type = PR_TYPE_APR; 613 } 614 615 if (ret) { 616 dev_err(dev, "Domain ID not specified in DT\n"); 617 return ret; 618 } 619 620 dev_set_drvdata(dev, apr); 621 apr->ch = rpdev->ept; 622 apr->dev = dev; 623 apr->rxwq = create_singlethread_workqueue("qcom_apr_rx"); 624 if (!apr->rxwq) { 625 dev_err(apr->dev, "Failed to start Rx WQ\n"); 626 return -ENOMEM; 627 } 628 INIT_WORK(&apr->rx_work, apr_rxwq); 629 630 apr->pdr = pdr_handle_alloc(apr_pd_status, apr); 631 if (IS_ERR(apr->pdr)) { 632 dev_err(dev, "Failed to init PDR handle\n"); 633 ret = PTR_ERR(apr->pdr); 634 goto destroy_wq; 635 } 636 637 INIT_LIST_HEAD(&apr->rx_list); 638 spin_lock_init(&apr->rx_lock); 639 spin_lock_init(&apr->svcs_lock); 640 idr_init(&apr->svcs_idr); 641 642 ret = of_apr_add_pd_lookups(dev); 643 if (ret) 644 goto handle_release; 645 646 of_register_apr_devices(dev, NULL); 647 648 return 0; 649 650handle_release: 651 pdr_handle_release(apr->pdr); 652destroy_wq: 653 destroy_workqueue(apr->rxwq); 654 return ret; 655} 656 657static void apr_remove(struct rpmsg_device *rpdev) 658{ 659 struct packet_router *apr = dev_get_drvdata(&rpdev->dev); 660 661 pdr_handle_release(apr->pdr); 662 device_for_each_child(&rpdev->dev, NULL, apr_remove_device); 663 destroy_workqueue(apr->rxwq); 664} 665 666/* 667 * __apr_driver_register() - Client driver registration with aprbus 668 * 669 * @drv:Client driver to be associated with client-device. 670 * @owner: owning module/driver 671 * 672 * This API will register the client driver with the aprbus 673 * It is called from the driver's module-init function. 674 */ 675int __apr_driver_register(struct apr_driver *drv, struct module *owner) 676{ 677 drv->driver.bus = &aprbus; 678 drv->driver.owner = owner; 679 680 return driver_register(&drv->driver); 681} 682EXPORT_SYMBOL_GPL(__apr_driver_register); 683 684/* 685 * apr_driver_unregister() - Undo effect of apr_driver_register 686 * 687 * @drv: Client driver to be unregistered 688 */ 689void apr_driver_unregister(struct apr_driver *drv) 690{ 691 driver_unregister(&drv->driver); 692} 693EXPORT_SYMBOL_GPL(apr_driver_unregister); 694 695static const struct of_device_id pkt_router_of_match[] = { 696 { .compatible = "qcom,apr"}, 697 { .compatible = "qcom,apr-v2"}, 698 { .compatible = "qcom,gpr"}, 699 {} 700}; 701MODULE_DEVICE_TABLE(of, pkt_router_of_match); 702 703static struct rpmsg_driver packet_router_driver = { 704 .probe = apr_probe, 705 .remove = apr_remove, 706 .callback = apr_callback, 707 .drv = { 708 .name = "qcom,apr", 709 .of_match_table = pkt_router_of_match, 710 }, 711}; 712 713static int __init apr_init(void) 714{ 715 int ret; 716 717 ret = bus_register(&aprbus); 718 if (!ret) 719 ret = register_rpmsg_driver(&packet_router_driver); 720 else 721 bus_unregister(&aprbus); 722 723 return ret; 724} 725 726static void __exit apr_exit(void) 727{ 728 bus_unregister(&aprbus); 729 unregister_rpmsg_driver(&packet_router_driver); 730} 731 732subsys_initcall(apr_init); 733module_exit(apr_exit); 734 735MODULE_LICENSE("GPL v2"); 736MODULE_DESCRIPTION("Qualcomm APR Bus"); 737