drivers/soc/qcom/apr.c

   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 };
  21
  22 struct 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
  36 struct 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  */
  50 int 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 }
  70 EXPORT_SYMBOL_GPL(apr_send_pkt);
  71
  72 static void apr_dev_release(struct device *dev)
  73 {
  74         struct apr_device *adev = to_apr_device(dev);
  75
  76         kfree(adev);
  77 }
  78
  79 static 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
 108 static 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
 180 static 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
 203 static 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
 226 static 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
 234 static 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
 252 static 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
 264 struct 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 };
 271 EXPORT_SYMBOL_GPL(aprbus);
 272
 273 static 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
 338 out:
 339         return ret;
 340 }
 341
 342 static 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
 375 static 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
 424 static 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
 438 static 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
 452 static 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
 502 handle_release:
 503         pdr_handle_release(apr->pdr);
 504 destroy_wq:
 505         destroy_workqueue(apr->rxwq);
 506         return ret;
 507 }
 508
 509 static 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  */
 528 int __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 }
 535 EXPORT_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  */
 542 void apr_driver_unregister(struct apr_driver *drv)
 543 {
 544         driver_unregister(&drv->driver);
 545 }
 546 EXPORT_SYMBOL_GPL(apr_driver_unregister);
 547
 548 static const struct of_device_id pkt_router_of_match[] = {
 549         { .compatible = "qcom,apr"},
 550         { .compatible = "qcom,apr-v2"},
 551         {}
 552 };
 553 MODULE_DEVICE_TABLE(of, pkt_router_of_match);
 554
 555 static 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
 565 static 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
 578 static void __exit apr_exit(void)
 579 {
 580         bus_unregister(&aprbus);
 581         unregister_rpmsg_driver(&packet_router_driver);
 582 }
 583
 584 subsys_initcall(apr_init);
 585 module_exit(apr_exit);
 586
 587 MODULE_LICENSE("GPL v2");
 588 MODULE_DESCRIPTION("Qualcomm APR Bus");