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