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