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}; 21 22struct packet_router { 23 struct rpmsg_endpoint *ch; 24 struct device *dev; 25 spinlock_t svcs_lock; 26 spinlock_t rx_lock; 27 struct idr svcs_idr; 28 int dest_domain_id; 29 int type; 30 struct pdr_handle *pdr; 31 struct workqueue_struct *rxwq; 32 struct work_struct rx_work; 33 struct list_head rx_list; 34}; 35 36struct apr_rx_buf { 37 struct list_head node; 38 int len; 39 uint8_t buf[]; 40}; 41 42/** 43 * apr_send_pkt() - Send a apr message from apr device 44 * 45 * @adev: Pointer to previously registered apr device. 46 * @pkt: Pointer to apr packet to send 47 * 48 * Return: Will be an negative on packet size on success. 49 */ 50int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt) 51{ 52 struct packet_router *apr = dev_get_drvdata(adev->dev.parent); 53 struct apr_hdr *hdr; 54 unsigned long flags; 55 int ret; 56 57 spin_lock_irqsave(&adev->svc.lock, flags); 58 59 hdr = &pkt->hdr; 60 hdr->src_domain = APR_DOMAIN_APPS; 61 hdr->src_svc = adev->svc.id; 62 hdr->dest_domain = adev->domain_id; 63 hdr->dest_svc = adev->svc.id; 64 65 ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size); 66 spin_unlock_irqrestore(&adev->svc.lock, flags); 67 68 return ret ? ret : hdr->pkt_size; 69} 70EXPORT_SYMBOL_GPL(apr_send_pkt); 71 72static void apr_dev_release(struct device *dev) 73{ 74 struct apr_device *adev = to_apr_device(dev); 75 76 kfree(adev); 77} 78 79static int apr_callback(struct rpmsg_device *rpdev, void *buf, 80 int len, void *priv, u32 addr) 81{ 82 struct packet_router *apr = dev_get_drvdata(&rpdev->dev); 83 struct apr_rx_buf *abuf; 84 unsigned long flags; 85 86 if (len <= APR_HDR_SIZE) { 87 dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n", 88 buf, len); 89 return -EINVAL; 90 } 91 92 abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC); 93 if (!abuf) 94 return -ENOMEM; 95 96 abuf->len = len; 97 memcpy(abuf->buf, buf, len); 98 99 spin_lock_irqsave(&apr->rx_lock, flags); 100 list_add_tail(&abuf->node, &apr->rx_list); 101 spin_unlock_irqrestore(&apr->rx_lock, flags); 102 103 queue_work(apr->rxwq, &apr->rx_work); 104 105 return 0; 106} 107 108static int apr_do_rx_callback(struct packet_router *apr, struct apr_rx_buf *abuf) 109{ 110 uint16_t hdr_size, msg_type, ver, svc_id; 111 struct pkt_router_svc *svc; 112 struct apr_device *adev; 113 struct apr_driver *adrv = NULL; 114 struct apr_resp_pkt resp; 115 struct apr_hdr *hdr; 116 unsigned long flags; 117 void *buf = abuf->buf; 118 int len = abuf->len; 119 120 hdr = buf; 121 ver = APR_HDR_FIELD_VER(hdr->hdr_field); 122 if (ver > APR_PKT_VER + 1) 123 return -EINVAL; 124 125 hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field); 126 if (hdr_size < APR_HDR_SIZE) { 127 dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size); 128 return -EINVAL; 129 } 130 131 if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) { 132 dev_err(apr->dev, "APR: Wrong packet size\n"); 133 return -EINVAL; 134 } 135 136 msg_type = APR_HDR_FIELD_MT(hdr->hdr_field); 137 if (msg_type >= APR_MSG_TYPE_MAX) { 138 dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type); 139 return -EINVAL; 140 } 141 142 if (hdr->src_domain >= APR_DOMAIN_MAX || 143 hdr->dest_domain >= APR_DOMAIN_MAX || 144 hdr->src_svc >= APR_SVC_MAX || 145 hdr->dest_svc >= APR_SVC_MAX) { 146 dev_err(apr->dev, "APR: Wrong APR header\n"); 147 return -EINVAL; 148 } 149 150 svc_id = hdr->dest_svc; 151 spin_lock_irqsave(&apr->svcs_lock, flags); 152 svc = idr_find(&apr->svcs_idr, svc_id); 153 if (svc && svc->dev->driver) { 154 adev = svc_to_apr_device(svc); 155 adrv = to_apr_driver(adev->dev.driver); 156 } 157 spin_unlock_irqrestore(&apr->svcs_lock, flags); 158 159 if (!adrv || !adev) { 160 dev_err(apr->dev, "APR: service is not registered (%d)\n", 161 svc_id); 162 return -EINVAL; 163 } 164 165 resp.hdr = *hdr; 166 resp.payload_size = hdr->pkt_size - hdr_size; 167 168 /* 169 * NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include 170 * optional headers in to apr_hdr which should be ignored 171 */ 172 if (resp.payload_size > 0) 173 resp.payload = buf + hdr_size; 174 175 adrv->callback(adev, &resp); 176 177 return 0; 178} 179 180static void apr_rxwq(struct work_struct *work) 181{ 182 struct packet_router *apr = container_of(work, struct packet_router, rx_work); 183 struct apr_rx_buf *abuf, *b; 184 unsigned long flags; 185 186 if (!list_empty(&apr->rx_list)) { 187 list_for_each_entry_safe(abuf, b, &apr->rx_list, node) { 188 switch (apr->type) { 189 case PR_TYPE_APR: 190 apr_do_rx_callback(apr, abuf); 191 break; 192 default: 193 break; 194 } 195 spin_lock_irqsave(&apr->rx_lock, flags); 196 list_del(&abuf->node); 197 spin_unlock_irqrestore(&apr->rx_lock, flags); 198 kfree(abuf); 199 } 200 } 201} 202 203static int apr_device_match(struct device *dev, struct device_driver *drv) 204{ 205 struct apr_device *adev = to_apr_device(dev); 206 struct apr_driver *adrv = to_apr_driver(drv); 207 const struct apr_device_id *id = adrv->id_table; 208 209 /* Attempt an OF style match first */ 210 if (of_driver_match_device(dev, drv)) 211 return 1; 212 213 if (!id) 214 return 0; 215 216 while (id->domain_id != 0 || id->svc_id != 0) { 217 if (id->domain_id == adev->domain_id && 218 id->svc_id == adev->svc.id) 219 return 1; 220 id++; 221 } 222 223 return 0; 224} 225 226static int apr_device_probe(struct device *dev) 227{ 228 struct apr_device *adev = to_apr_device(dev); 229 struct apr_driver *adrv = to_apr_driver(dev->driver); 230 231 return adrv->probe(adev); 232} 233 234static int apr_device_remove(struct device *dev) 235{ 236 struct apr_device *adev = to_apr_device(dev); 237 struct apr_driver *adrv; 238 struct packet_router *apr = dev_get_drvdata(adev->dev.parent); 239 240 if (dev->driver) { 241 adrv = to_apr_driver(dev->driver); 242 if (adrv->remove) 243 adrv->remove(adev); 244 spin_lock(&apr->svcs_lock); 245 idr_remove(&apr->svcs_idr, adev->svc.id); 246 spin_unlock(&apr->svcs_lock); 247 } 248 249 return 0; 250} 251 252static int apr_uevent(struct device *dev, struct kobj_uevent_env *env) 253{ 254 struct apr_device *adev = to_apr_device(dev); 255 int ret; 256 257 ret = of_device_uevent_modalias(dev, env); 258 if (ret != -ENODEV) 259 return ret; 260 261 return add_uevent_var(env, "MODALIAS=apr:%s", adev->name); 262} 263 264struct bus_type aprbus = { 265 .name = "aprbus", 266 .match = apr_device_match, 267 .probe = apr_device_probe, 268 .uevent = apr_uevent, 269 .remove = apr_device_remove, 270}; 271EXPORT_SYMBOL_GPL(aprbus); 272 273static int apr_add_device(struct device *dev, struct device_node *np, 274 u32 svc_id, u32 domain_id) 275{ 276 struct packet_router *apr = dev_get_drvdata(dev); 277 struct apr_device *adev = NULL; 278 struct pkt_router_svc *svc; 279 int ret; 280 281 adev = kzalloc(sizeof(*adev), GFP_KERNEL); 282 if (!adev) 283 return -ENOMEM; 284 285 adev->svc_id = svc_id; 286 svc = &adev->svc; 287 288 svc->id = svc_id; 289 svc->pr = apr; 290 svc->priv = adev; 291 svc->dev = dev; 292 spin_lock_init(&svc->lock); 293 294 adev->domain_id = domain_id; 295 296 if (np) 297 snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np); 298 299 switch (apr->type) { 300 case PR_TYPE_APR: 301 dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name, 302 domain_id, svc_id); 303 break; 304 default: 305 break; 306 } 307 308 adev->dev.bus = &aprbus; 309 adev->dev.parent = dev; 310 adev->dev.of_node = np; 311 adev->dev.release = apr_dev_release; 312 adev->dev.driver = NULL; 313 314 spin_lock(&apr->svcs_lock); 315 ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC); 316 spin_unlock(&apr->svcs_lock); 317 if (ret < 0) { 318 dev_err(dev, "idr_alloc failed: %d\n", ret); 319 goto out; 320 } 321 322 /* Protection domain is optional, it does not exist on older platforms */ 323 ret = of_property_read_string_index(np, "qcom,protection-domain", 324 1, &adev->service_path); 325 if (ret < 0 && ret != -EINVAL) { 326 dev_err(dev, "Failed to read second value of qcom,protection-domain\n"); 327 goto out; 328 } 329 330 dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev)); 331 332 ret = device_register(&adev->dev); 333 if (ret) { 334 dev_err(dev, "device_register failed: %d\n", ret); 335 put_device(&adev->dev); 336 } 337 338out: 339 return ret; 340} 341 342static int of_apr_add_pd_lookups(struct device *dev) 343{ 344 const char *service_name, *service_path; 345 struct packet_router *apr = dev_get_drvdata(dev); 346 struct device_node *node; 347 struct pdr_service *pds; 348 int ret; 349 350 for_each_child_of_node(dev->of_node, node) { 351 ret = of_property_read_string_index(node, "qcom,protection-domain", 352 0, &service_name); 353 if (ret < 0) 354 continue; 355 356 ret = of_property_read_string_index(node, "qcom,protection-domain", 357 1, &service_path); 358 if (ret < 0) { 359 dev_err(dev, "pdr service path missing: %d\n", ret); 360 of_node_put(node); 361 return ret; 362 } 363 364 pds = pdr_add_lookup(apr->pdr, service_name, service_path); 365 if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) { 366 dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds)); 367 of_node_put(node); 368 return PTR_ERR(pds); 369 } 370 } 371 372 return 0; 373} 374 375static void of_register_apr_devices(struct device *dev, const char *svc_path) 376{ 377 struct packet_router *apr = dev_get_drvdata(dev); 378 struct device_node *node; 379 const char *service_path; 380 int ret; 381 382 for_each_child_of_node(dev->of_node, node) { 383 u32 svc_id; 384 u32 domain_id; 385 386 /* 387 * This function is called with svc_path NULL during 388 * apr_probe(), in which case we register any apr devices 389 * without a qcom,protection-domain specified. 390 * 391 * Then as the protection domains becomes available 392 * (if applicable) this function is again called, but with 393 * svc_path representing the service becoming available. In 394 * this case we register any apr devices with a matching 395 * qcom,protection-domain. 396 */ 397 398 ret = of_property_read_string_index(node, "qcom,protection-domain", 399 1, &service_path); 400 if (svc_path) { 401 /* skip APR services that are PD independent */ 402 if (ret) 403 continue; 404 405 /* skip APR services whose PD paths don't match */ 406 if (strcmp(service_path, svc_path)) 407 continue; 408 } else { 409 /* skip APR services whose PD lookups are registered */ 410 if (ret == 0) 411 continue; 412 } 413 414 if (of_property_read_u32(node, "reg", &svc_id)) 415 continue; 416 417 domain_id = apr->dest_domain_id; 418 419 if (apr_add_device(dev, node, svc_id, domain_id)) 420 dev_err(dev, "Failed to add apr %d svc\n", svc_id); 421 } 422} 423 424static int apr_remove_device(struct device *dev, void *svc_path) 425{ 426 struct apr_device *adev = to_apr_device(dev); 427 428 if (svc_path && adev->service_path) { 429 if (!strcmp(adev->service_path, (char *)svc_path)) 430 device_unregister(&adev->dev); 431 } else { 432 device_unregister(&adev->dev); 433 } 434 435 return 0; 436} 437 438static void apr_pd_status(int state, char *svc_path, void *priv) 439{ 440 struct packet_router *apr = (struct packet_router *)priv; 441 442 switch (state) { 443 case SERVREG_SERVICE_STATE_UP: 444 of_register_apr_devices(apr->dev, svc_path); 445 break; 446 case SERVREG_SERVICE_STATE_DOWN: 447 device_for_each_child(apr->dev, svc_path, apr_remove_device); 448 break; 449 } 450} 451 452static int apr_probe(struct rpmsg_device *rpdev) 453{ 454 struct device *dev = &rpdev->dev; 455 struct packet_router *apr; 456 int ret; 457 458 apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL); 459 if (!apr) 460 return -ENOMEM; 461 462 ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id); 463 if (ret) /* try deprecated apr-domain property */ 464 ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", 465 &apr->dest_domain_id); 466 apr->type = PR_TYPE_APR; 467 if (ret) { 468 dev_err(dev, "Domain ID not specified in DT\n"); 469 return ret; 470 } 471 472 dev_set_drvdata(dev, apr); 473 apr->ch = rpdev->ept; 474 apr->dev = dev; 475 apr->rxwq = create_singlethread_workqueue("qcom_apr_rx"); 476 if (!apr->rxwq) { 477 dev_err(apr->dev, "Failed to start Rx WQ\n"); 478 return -ENOMEM; 479 } 480 INIT_WORK(&apr->rx_work, apr_rxwq); 481 482 apr->pdr = pdr_handle_alloc(apr_pd_status, apr); 483 if (IS_ERR(apr->pdr)) { 484 dev_err(dev, "Failed to init PDR handle\n"); 485 ret = PTR_ERR(apr->pdr); 486 goto destroy_wq; 487 } 488 489 INIT_LIST_HEAD(&apr->rx_list); 490 spin_lock_init(&apr->rx_lock); 491 spin_lock_init(&apr->svcs_lock); 492 idr_init(&apr->svcs_idr); 493 494 ret = of_apr_add_pd_lookups(dev); 495 if (ret) 496 goto handle_release; 497 498 of_register_apr_devices(dev, NULL); 499 500 return 0; 501 502handle_release: 503 pdr_handle_release(apr->pdr); 504destroy_wq: 505 destroy_workqueue(apr->rxwq); 506 return ret; 507} 508 509static void apr_remove(struct rpmsg_device *rpdev) 510{ 511 struct packet_router *apr = dev_get_drvdata(&rpdev->dev); 512 513 pdr_handle_release(apr->pdr); 514 device_for_each_child(&rpdev->dev, NULL, apr_remove_device); 515 flush_workqueue(apr->rxwq); 516 destroy_workqueue(apr->rxwq); 517} 518 519/* 520 * __apr_driver_register() - Client driver registration with aprbus 521 * 522 * @drv:Client driver to be associated with client-device. 523 * @owner: owning module/driver 524 * 525 * This API will register the client driver with the aprbus 526 * It is called from the driver's module-init function. 527 */ 528int __apr_driver_register(struct apr_driver *drv, struct module *owner) 529{ 530 drv->driver.bus = &aprbus; 531 drv->driver.owner = owner; 532 533 return driver_register(&drv->driver); 534} 535EXPORT_SYMBOL_GPL(__apr_driver_register); 536 537/* 538 * apr_driver_unregister() - Undo effect of apr_driver_register 539 * 540 * @drv: Client driver to be unregistered 541 */ 542void apr_driver_unregister(struct apr_driver *drv) 543{ 544 driver_unregister(&drv->driver); 545} 546EXPORT_SYMBOL_GPL(apr_driver_unregister); 547 548static const struct of_device_id pkt_router_of_match[] = { 549 { .compatible = "qcom,apr"}, 550 { .compatible = "qcom,apr-v2"}, 551 {} 552}; 553MODULE_DEVICE_TABLE(of, pkt_router_of_match); 554 555static struct rpmsg_driver packet_router_driver = { 556 .probe = apr_probe, 557 .remove = apr_remove, 558 .callback = apr_callback, 559 .drv = { 560 .name = "qcom,apr", 561 .of_match_table = pkt_router_of_match, 562 }, 563}; 564 565static int __init apr_init(void) 566{ 567 int ret; 568 569 ret = bus_register(&aprbus); 570 if (!ret) 571 ret = register_rpmsg_driver(&packet_router_driver); 572 else 573 bus_unregister(&aprbus); 574 575 return ret; 576} 577 578static void __exit apr_exit(void) 579{ 580 bus_unregister(&aprbus); 581 unregister_rpmsg_driver(&packet_router_driver); 582} 583 584subsys_initcall(apr_init); 585module_exit(apr_exit); 586 587MODULE_LICENSE("GPL v2"); 588MODULE_DESCRIPTION("Qualcomm APR Bus"); 589