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 18 enum { 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 27 struct 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 41 struct 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 */ 55 int 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 } 75 EXPORT_SYMBOL_GPL(apr_send_pkt); 76 77 void 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 } 88 EXPORT_SYMBOL_GPL(gpr_free_port); 89 90 gpr_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_obj(*port); 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 } 124 EXPORT_SYMBOL_GPL(gpr_alloc_port); 125 126 static int pkt_router_send_svc_pkt(struct pkt_router_svc *svc, const struct gpr_pkt *pkt) 127 { 128 struct packet_router *pr = svc->pr; 129 const 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 142 int gpr_send_pkt(struct apr_device *gdev, const struct gpr_pkt *pkt) 143 { 144 return pkt_router_send_svc_pkt(&gdev->svc, pkt); 145 } 146 EXPORT_SYMBOL_GPL(gpr_send_pkt); 147 148 int gpr_send_port_pkt(gpr_port_t *port, const struct gpr_pkt *pkt) 149 { 150 return pkt_router_send_svc_pkt(port, pkt); 151 } 152 EXPORT_SYMBOL_GPL(gpr_send_port_pkt); 153 154 static void apr_dev_release(struct device *dev) 155 { 156 struct apr_device *adev = to_apr_device(dev); 157 158 kfree(adev); 159 } 160 161 static 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_flex(*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 190 static 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 262 static 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 315 static 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 341 static int apr_device_probe(struct device *dev) 342 { 343 struct apr_device *adev = to_apr_device(dev); 344 struct apr_driver *adrv = to_apr_driver(dev->driver); 345 int ret; 346 347 ret = adrv->probe(adev); 348 if (!ret) 349 adev->svc.callback = adrv->gpr_callback; 350 351 return ret; 352 } 353 354 static void apr_device_remove(struct device *dev) 355 { 356 struct apr_device *adev = to_apr_device(dev); 357 struct apr_driver *adrv = to_apr_driver(dev->driver); 358 struct packet_router *apr = dev_get_drvdata(adev->dev.parent); 359 360 if (adrv->remove) 361 adrv->remove(adev); 362 spin_lock(&apr->svcs_lock); 363 idr_remove(&apr->svcs_idr, adev->svc.id); 364 spin_unlock(&apr->svcs_lock); 365 } 366 367 static int apr_uevent(const struct device *dev, struct kobj_uevent_env *env) 368 { 369 const struct apr_device *adev = to_apr_device(dev); 370 int ret; 371 372 ret = of_device_uevent_modalias(dev, env); 373 if (ret != -ENODEV) 374 return ret; 375 376 return add_uevent_var(env, "MODALIAS=apr:%s", adev->name); 377 } 378 379 const struct bus_type aprbus = { 380 .name = "aprbus", 381 .match = of_driver_match_device, 382 .probe = apr_device_probe, 383 .uevent = apr_uevent, 384 .remove = apr_device_remove, 385 }; 386 EXPORT_SYMBOL_GPL(aprbus); 387 388 static int apr_add_device(struct device *dev, struct device_node *np, 389 u32 svc_id, u32 domain_id) 390 { 391 struct packet_router *apr = dev_get_drvdata(dev); 392 struct apr_device *adev = NULL; 393 struct pkt_router_svc *svc; 394 int ret; 395 396 adev = kzalloc_obj(*adev); 397 if (!adev) 398 return -ENOMEM; 399 400 adev->svc_id = svc_id; 401 svc = &adev->svc; 402 403 svc->id = svc_id; 404 svc->pr = apr; 405 svc->priv = adev; 406 svc->dev = dev; 407 spin_lock_init(&svc->lock); 408 409 adev->domain_id = domain_id; 410 411 if (np) 412 snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np); 413 414 adev->dev.bus = &aprbus; 415 adev->dev.parent = dev; 416 adev->dev.of_node = np; 417 adev->dev.release = apr_dev_release; 418 adev->dev.driver = NULL; 419 device_initialize(&adev->dev); 420 421 switch (apr->type) { 422 case PR_TYPE_APR: 423 ret = dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name, 424 domain_id, svc_id); 425 break; 426 case PR_TYPE_GPR: 427 ret = dev_set_name(&adev->dev, "gprsvc:%s:%x:%x", adev->name, 428 domain_id, svc_id); 429 break; 430 default: 431 ret = -EINVAL; 432 break; 433 } 434 if (ret) 435 goto out_put_device; 436 437 spin_lock(&apr->svcs_lock); 438 ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC); 439 spin_unlock(&apr->svcs_lock); 440 if (ret < 0) { 441 dev_err(dev, "idr_alloc failed: %d\n", ret); 442 goto out_put_device; 443 } 444 445 /* Protection domain is optional, it does not exist on older platforms */ 446 ret = of_property_read_string_index(np, "qcom,protection-domain", 447 1, &adev->service_path); 448 if (ret < 0 && ret != -EINVAL) { 449 dev_err(dev, "Failed to read second value of qcom,protection-domain\n"); 450 goto out_remove_idr; 451 } 452 453 dev_info(dev, "Adding APR/GPR dev: %s\n", dev_name(&adev->dev)); 454 455 ret = device_add(&adev->dev); 456 if (ret) { 457 dev_err(dev, "device_add failed: %d\n", ret); 458 goto out_remove_idr; 459 } 460 461 return 0; 462 463 out_remove_idr: 464 spin_lock(&apr->svcs_lock); 465 idr_remove(&apr->svcs_idr, svc_id); 466 spin_unlock(&apr->svcs_lock); 467 flush_workqueue(apr->rxwq); 468 out_put_device: 469 put_device(&adev->dev); 470 return ret; 471 } 472 473 static int of_apr_add_pd_lookups(struct device *dev) 474 { 475 const char *service_name, *service_path; 476 struct packet_router *apr = dev_get_drvdata(dev); 477 struct pdr_service *pds; 478 int ret; 479 480 for_each_child_of_node_scoped(dev->of_node, node) { 481 ret = of_property_read_string_index(node, "qcom,protection-domain", 482 0, &service_name); 483 if (ret < 0) 484 continue; 485 486 ret = of_property_read_string_index(node, "qcom,protection-domain", 487 1, &service_path); 488 if (ret < 0) { 489 dev_err(dev, "pdr service path missing: %d\n", ret); 490 return ret; 491 } 492 493 pds = pdr_add_lookup(apr->pdr, service_name, service_path); 494 if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) { 495 dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds)); 496 return PTR_ERR(pds); 497 } 498 } 499 500 return 0; 501 } 502 503 static void of_register_apr_devices(struct device *dev, const char *svc_path) 504 { 505 struct packet_router *apr = dev_get_drvdata(dev); 506 struct device_node *node; 507 const char *service_path; 508 int ret; 509 510 for_each_child_of_node(dev->of_node, node) { 511 u32 svc_id; 512 u32 domain_id; 513 514 /* 515 * This function is called with svc_path NULL during 516 * apr_probe(), in which case we register any apr devices 517 * without a qcom,protection-domain specified. 518 * 519 * Then as the protection domains becomes available 520 * (if applicable) this function is again called, but with 521 * svc_path representing the service becoming available. In 522 * this case we register any apr devices with a matching 523 * qcom,protection-domain. 524 */ 525 526 ret = of_property_read_string_index(node, "qcom,protection-domain", 527 1, &service_path); 528 if (svc_path) { 529 /* skip APR services that are PD independent */ 530 if (ret) 531 continue; 532 533 /* skip APR services whose PD paths don't match */ 534 if (strcmp(service_path, svc_path)) 535 continue; 536 } else { 537 /* skip APR services whose PD lookups are registered */ 538 if (ret == 0) 539 continue; 540 } 541 542 if (of_property_read_u32(node, "reg", &svc_id)) 543 continue; 544 545 domain_id = apr->dest_domain_id; 546 547 if (apr_add_device(dev, node, svc_id, domain_id)) 548 dev_err(dev, "Failed to add apr %d svc\n", svc_id); 549 } 550 } 551 552 static int apr_remove_device(struct device *dev, void *svc_path) 553 { 554 struct apr_device *adev = to_apr_device(dev); 555 556 if (svc_path && adev->service_path) { 557 if (!strcmp(adev->service_path, (char *)svc_path)) 558 device_unregister(&adev->dev); 559 } else { 560 device_unregister(&adev->dev); 561 } 562 563 return 0; 564 } 565 566 static void apr_pd_status(int state, char *svc_path, void *priv) 567 { 568 struct packet_router *apr = (struct packet_router *)priv; 569 570 switch (state) { 571 case SERVREG_SERVICE_STATE_UP: 572 of_register_apr_devices(apr->dev, svc_path); 573 break; 574 case SERVREG_SERVICE_STATE_DOWN: 575 device_for_each_child(apr->dev, svc_path, apr_remove_device); 576 break; 577 } 578 } 579 580 static int apr_probe(struct rpmsg_device *rpdev) 581 { 582 struct device *dev = &rpdev->dev; 583 struct packet_router *apr; 584 int ret; 585 586 apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL); 587 if (!apr) 588 return -ENOMEM; 589 590 ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id); 591 592 if (of_device_is_compatible(dev->of_node, "qcom,gpr")) { 593 apr->type = PR_TYPE_GPR; 594 } else { 595 if (ret) /* try deprecated apr-domain property */ 596 ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", 597 &apr->dest_domain_id); 598 apr->type = PR_TYPE_APR; 599 } 600 601 if (ret) { 602 dev_err(dev, "Domain ID not specified in DT\n"); 603 return ret; 604 } 605 606 dev_set_drvdata(dev, apr); 607 apr->ch = rpdev->ept; 608 apr->dev = dev; 609 apr->rxwq = create_singlethread_workqueue("qcom_apr_rx"); 610 if (!apr->rxwq) { 611 dev_err(apr->dev, "Failed to start Rx WQ\n"); 612 return -ENOMEM; 613 } 614 INIT_WORK(&apr->rx_work, apr_rxwq); 615 616 apr->pdr = pdr_handle_alloc(apr_pd_status, apr); 617 if (IS_ERR(apr->pdr)) { 618 dev_err(dev, "Failed to init PDR handle\n"); 619 ret = PTR_ERR(apr->pdr); 620 goto destroy_wq; 621 } 622 623 INIT_LIST_HEAD(&apr->rx_list); 624 spin_lock_init(&apr->rx_lock); 625 spin_lock_init(&apr->svcs_lock); 626 idr_init(&apr->svcs_idr); 627 628 ret = of_apr_add_pd_lookups(dev); 629 if (ret) 630 goto handle_release; 631 632 of_register_apr_devices(dev, NULL); 633 634 return 0; 635 636 handle_release: 637 pdr_handle_release(apr->pdr); 638 destroy_wq: 639 destroy_workqueue(apr->rxwq); 640 return ret; 641 } 642 643 static void apr_remove(struct rpmsg_device *rpdev) 644 { 645 struct packet_router *apr = dev_get_drvdata(&rpdev->dev); 646 647 pdr_handle_release(apr->pdr); 648 device_for_each_child(&rpdev->dev, NULL, apr_remove_device); 649 destroy_workqueue(apr->rxwq); 650 } 651 652 /* 653 * __apr_driver_register() - Client driver registration with aprbus 654 * 655 * @drv:Client driver to be associated with client-device. 656 * @owner: owning module/driver 657 * 658 * This API will register the client driver with the aprbus 659 * It is called from the driver's module-init function. 660 */ 661 int __apr_driver_register(struct apr_driver *drv, struct module *owner) 662 { 663 drv->driver.bus = &aprbus; 664 drv->driver.owner = owner; 665 666 return driver_register(&drv->driver); 667 } 668 EXPORT_SYMBOL_GPL(__apr_driver_register); 669 670 /* 671 * apr_driver_unregister() - Undo effect of apr_driver_register 672 * 673 * @drv: Client driver to be unregistered 674 */ 675 void apr_driver_unregister(struct apr_driver *drv) 676 { 677 driver_unregister(&drv->driver); 678 } 679 EXPORT_SYMBOL_GPL(apr_driver_unregister); 680 681 static const struct of_device_id pkt_router_of_match[] = { 682 { .compatible = "qcom,apr"}, 683 { .compatible = "qcom,apr-v2"}, 684 { .compatible = "qcom,gpr"}, 685 {} 686 }; 687 MODULE_DEVICE_TABLE(of, pkt_router_of_match); 688 689 static struct rpmsg_driver packet_router_driver = { 690 .probe = apr_probe, 691 .remove = apr_remove, 692 .callback = apr_callback, 693 .drv = { 694 .name = "qcom,apr", 695 .of_match_table = pkt_router_of_match, 696 }, 697 }; 698 699 static int __init apr_init(void) 700 { 701 int ret; 702 703 ret = bus_register(&aprbus); 704 if (!ret) 705 ret = register_rpmsg_driver(&packet_router_driver); 706 else 707 bus_unregister(&aprbus); 708 709 return ret; 710 } 711 712 static void __exit apr_exit(void) 713 { 714 bus_unregister(&aprbus); 715 unregister_rpmsg_driver(&packet_router_driver); 716 } 717 718 subsys_initcall(apr_init); 719 module_exit(apr_exit); 720 721 MODULE_LICENSE("GPL v2"); 722 MODULE_DESCRIPTION("Qualcomm APR Bus"); 723