linux/drivers/soc/qcom/apr.c
<<
>>
Prefs
   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
  18struct 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
  31struct 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 */
  45int 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}
  65EXPORT_SYMBOL_GPL(apr_send_pkt);
  66
  67static void apr_dev_release(struct device *dev)
  68{
  69        struct apr_device *adev = to_apr_device(dev);
  70
  71        kfree(adev);
  72}
  73
  74static 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
 104static 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
 172static 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
 189static 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
 212static 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
 220static 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
 238static 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
 250struct 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};
 257EXPORT_SYMBOL_GPL(aprbus);
 258
 259static 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
 308static 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                        return ret;
 327                }
 328
 329                pds = pdr_add_lookup(apr->pdr, service_name, service_path);
 330                if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
 331                        dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));
 332                        return PTR_ERR(pds);
 333                }
 334        }
 335
 336        return 0;
 337}
 338
 339static void of_register_apr_devices(struct device *dev, const char *svc_path)
 340{
 341        struct apr *apr = dev_get_drvdata(dev);
 342        struct device_node *node;
 343        const char *service_path;
 344        int ret;
 345
 346        for_each_child_of_node(dev->of_node, node) {
 347                struct apr_device_id id = { {0} };
 348
 349                /*
 350                 * This function is called with svc_path NULL during
 351                 * apr_probe(), in which case we register any apr devices
 352                 * without a qcom,protection-domain specified.
 353                 *
 354                 * Then as the protection domains becomes available
 355                 * (if applicable) this function is again called, but with
 356                 * svc_path representing the service becoming available. In
 357                 * this case we register any apr devices with a matching
 358                 * qcom,protection-domain.
 359                 */
 360
 361                ret = of_property_read_string_index(node, "qcom,protection-domain",
 362                                                    1, &service_path);
 363                if (svc_path) {
 364                        /* skip APR services that are PD independent */
 365                        if (ret)
 366                                continue;
 367
 368                        /* skip APR services whose PD paths don't match */
 369                        if (strcmp(service_path, svc_path))
 370                                continue;
 371                } else {
 372                        /* skip APR services whose PD lookups are registered */
 373                        if (ret == 0)
 374                                continue;
 375                }
 376
 377                if (of_property_read_u32(node, "reg", &id.svc_id))
 378                        continue;
 379
 380                id.domain_id = apr->dest_domain_id;
 381
 382                if (apr_add_device(dev, node, &id))
 383                        dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
 384        }
 385}
 386
 387static int apr_remove_device(struct device *dev, void *svc_path)
 388{
 389        struct apr_device *adev = to_apr_device(dev);
 390
 391        if (svc_path && adev->service_path) {
 392                if (!strcmp(adev->service_path, (char *)svc_path))
 393                        device_unregister(&adev->dev);
 394        } else {
 395                device_unregister(&adev->dev);
 396        }
 397
 398        return 0;
 399}
 400
 401static void apr_pd_status(int state, char *svc_path, void *priv)
 402{
 403        struct apr *apr = (struct apr *)priv;
 404
 405        switch (state) {
 406        case SERVREG_SERVICE_STATE_UP:
 407                of_register_apr_devices(apr->dev, svc_path);
 408                break;
 409        case SERVREG_SERVICE_STATE_DOWN:
 410                device_for_each_child(apr->dev, svc_path, apr_remove_device);
 411                break;
 412        }
 413}
 414
 415static int apr_probe(struct rpmsg_device *rpdev)
 416{
 417        struct device *dev = &rpdev->dev;
 418        struct apr *apr;
 419        int ret;
 420
 421        apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
 422        if (!apr)
 423                return -ENOMEM;
 424
 425        ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", &apr->dest_domain_id);
 426        if (ret) {
 427                dev_err(dev, "APR Domain ID not specified in DT\n");
 428                return ret;
 429        }
 430
 431        dev_set_drvdata(dev, apr);
 432        apr->ch = rpdev->ept;
 433        apr->dev = dev;
 434        apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
 435        if (!apr->rxwq) {
 436                dev_err(apr->dev, "Failed to start Rx WQ\n");
 437                return -ENOMEM;
 438        }
 439        INIT_WORK(&apr->rx_work, apr_rxwq);
 440
 441        apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
 442        if (IS_ERR(apr->pdr)) {
 443                dev_err(dev, "Failed to init PDR handle\n");
 444                ret = PTR_ERR(apr->pdr);
 445                goto destroy_wq;
 446        }
 447
 448        INIT_LIST_HEAD(&apr->rx_list);
 449        spin_lock_init(&apr->rx_lock);
 450        spin_lock_init(&apr->svcs_lock);
 451        idr_init(&apr->svcs_idr);
 452
 453        ret = of_apr_add_pd_lookups(dev);
 454        if (ret)
 455                goto handle_release;
 456
 457        of_register_apr_devices(dev, NULL);
 458
 459        return 0;
 460
 461handle_release:
 462        pdr_handle_release(apr->pdr);
 463destroy_wq:
 464        destroy_workqueue(apr->rxwq);
 465        return ret;
 466}
 467
 468static void apr_remove(struct rpmsg_device *rpdev)
 469{
 470        struct apr *apr = dev_get_drvdata(&rpdev->dev);
 471
 472        pdr_handle_release(apr->pdr);
 473        device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
 474        flush_workqueue(apr->rxwq);
 475        destroy_workqueue(apr->rxwq);
 476}
 477
 478/*
 479 * __apr_driver_register() - Client driver registration with aprbus
 480 *
 481 * @drv:Client driver to be associated with client-device.
 482 * @owner: owning module/driver
 483 *
 484 * This API will register the client driver with the aprbus
 485 * It is called from the driver's module-init function.
 486 */
 487int __apr_driver_register(struct apr_driver *drv, struct module *owner)
 488{
 489        drv->driver.bus = &aprbus;
 490        drv->driver.owner = owner;
 491
 492        return driver_register(&drv->driver);
 493}
 494EXPORT_SYMBOL_GPL(__apr_driver_register);
 495
 496/*
 497 * apr_driver_unregister() - Undo effect of apr_driver_register
 498 *
 499 * @drv: Client driver to be unregistered
 500 */
 501void apr_driver_unregister(struct apr_driver *drv)
 502{
 503        driver_unregister(&drv->driver);
 504}
 505EXPORT_SYMBOL_GPL(apr_driver_unregister);
 506
 507static const struct of_device_id apr_of_match[] = {
 508        { .compatible = "qcom,apr"},
 509        { .compatible = "qcom,apr-v2"},
 510        {}
 511};
 512MODULE_DEVICE_TABLE(of, apr_of_match);
 513
 514static struct rpmsg_driver apr_driver = {
 515        .probe = apr_probe,
 516        .remove = apr_remove,
 517        .callback = apr_callback,
 518        .drv = {
 519                .name = "qcom,apr",
 520                .of_match_table = apr_of_match,
 521        },
 522};
 523
 524static int __init apr_init(void)
 525{
 526        int ret;
 527
 528        ret = bus_register(&aprbus);
 529        if (!ret)
 530                ret = register_rpmsg_driver(&apr_driver);
 531        else
 532                bus_unregister(&aprbus);
 533
 534        return ret;
 535}
 536
 537static void __exit apr_exit(void)
 538{
 539        bus_unregister(&aprbus);
 540        unregister_rpmsg_driver(&apr_driver);
 541}
 542
 543subsys_initcall(apr_init);
 544module_exit(apr_exit);
 545
 546MODULE_LICENSE("GPL v2");
 547MODULE_DESCRIPTION("Qualcomm APR Bus");
 548