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"); 443