Loading...
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
17struct 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
29struct 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 */
43int 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}
63EXPORT_SYMBOL_GPL(apr_send_pkt);
64
65static void apr_dev_release(struct device *dev)
66{
67 struct apr_device *adev = to_apr_device(dev);
68
69 kfree(adev);
70}
71
72static 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
102static 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
170static 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
187static 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
210static 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
218static 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
236static 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
248struct 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};
255EXPORT_SYMBOL_GPL(aprbus);
256
257static 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
303static 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
321static 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
355static 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
364static 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 */
382int __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}
389EXPORT_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 */
396void apr_driver_unregister(struct apr_driver *drv)
397{
398 driver_unregister(&drv->driver);
399}
400EXPORT_SYMBOL_GPL(apr_driver_unregister);
401
402static const struct of_device_id apr_of_match[] = {
403 { .compatible = "qcom,apr"},
404 { .compatible = "qcom,apr-v2"},
405 {}
406};
407MODULE_DEVICE_TABLE(of, apr_of_match);
408
409static 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
419static 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
432static void __exit apr_exit(void)
433{
434 bus_unregister(&aprbus);
435 unregister_rpmsg_driver(&apr_driver);
436}
437
438subsys_initcall(apr_init);
439module_exit(apr_exit);
440
441MODULE_LICENSE("GPL v2");
442MODULE_DESCRIPTION("Qualcomm APR Bus");
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
18enum {
19 PR_TYPE_APR = 0,
20 PR_TYPE_GPR,
21};
22
23/* Some random values tbh which does not collide with static modules */
24#define GPR_DYNAMIC_PORT_START 0x10000000
25#define GPR_DYNAMIC_PORT_END 0x20000000
26
27struct packet_router {
28 struct rpmsg_endpoint *ch;
29 struct device *dev;
30 spinlock_t svcs_lock;
31 spinlock_t rx_lock;
32 struct idr svcs_idr;
33 int dest_domain_id;
34 int type;
35 struct pdr_handle *pdr;
36 struct workqueue_struct *rxwq;
37 struct work_struct rx_work;
38 struct list_head rx_list;
39};
40
41struct apr_rx_buf {
42 struct list_head node;
43 int len;
44 uint8_t buf[];
45};
46
47/**
48 * apr_send_pkt() - Send a apr message from apr device
49 *
50 * @adev: Pointer to previously registered apr device.
51 * @pkt: Pointer to apr packet to send
52 *
53 * Return: Will be an negative on packet size on success.
54 */
55int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt)
56{
57 struct packet_router *apr = dev_get_drvdata(adev->dev.parent);
58 struct apr_hdr *hdr;
59 unsigned long flags;
60 int ret;
61
62 spin_lock_irqsave(&adev->svc.lock, flags);
63
64 hdr = &pkt->hdr;
65 hdr->src_domain = APR_DOMAIN_APPS;
66 hdr->src_svc = adev->svc.id;
67 hdr->dest_domain = adev->domain_id;
68 hdr->dest_svc = adev->svc.id;
69
70 ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);
71 spin_unlock_irqrestore(&adev->svc.lock, flags);
72
73 return ret ? ret : hdr->pkt_size;
74}
75EXPORT_SYMBOL_GPL(apr_send_pkt);
76
77void gpr_free_port(gpr_port_t *port)
78{
79 struct packet_router *gpr = port->pr;
80 unsigned long flags;
81
82 spin_lock_irqsave(&gpr->svcs_lock, flags);
83 idr_remove(&gpr->svcs_idr, port->id);
84 spin_unlock_irqrestore(&gpr->svcs_lock, flags);
85
86 kfree(port);
87}
88EXPORT_SYMBOL_GPL(gpr_free_port);
89
90gpr_port_t *gpr_alloc_port(struct apr_device *gdev, struct device *dev,
91 gpr_port_cb cb, void *priv)
92{
93 struct packet_router *pr = dev_get_drvdata(gdev->dev.parent);
94 gpr_port_t *port;
95 struct pkt_router_svc *svc;
96 int id;
97
98 port = kzalloc(sizeof(*port), GFP_KERNEL);
99 if (!port)
100 return ERR_PTR(-ENOMEM);
101
102 svc = port;
103 svc->callback = cb;
104 svc->pr = pr;
105 svc->priv = priv;
106 svc->dev = dev;
107 spin_lock_init(&svc->lock);
108
109 spin_lock(&pr->svcs_lock);
110 id = idr_alloc_cyclic(&pr->svcs_idr, svc, GPR_DYNAMIC_PORT_START,
111 GPR_DYNAMIC_PORT_END, GFP_ATOMIC);
112 if (id < 0) {
113 dev_err(dev, "Unable to allocate dynamic GPR src port\n");
114 kfree(port);
115 spin_unlock(&pr->svcs_lock);
116 return ERR_PTR(id);
117 }
118
119 svc->id = id;
120 spin_unlock(&pr->svcs_lock);
121
122 return port;
123}
124EXPORT_SYMBOL_GPL(gpr_alloc_port);
125
126static int pkt_router_send_svc_pkt(struct pkt_router_svc *svc, struct gpr_pkt *pkt)
127{
128 struct packet_router *pr = svc->pr;
129 struct gpr_hdr *hdr;
130 unsigned long flags;
131 int ret;
132
133 hdr = &pkt->hdr;
134
135 spin_lock_irqsave(&svc->lock, flags);
136 ret = rpmsg_trysend(pr->ch, pkt, hdr->pkt_size);
137 spin_unlock_irqrestore(&svc->lock, flags);
138
139 return ret ? ret : hdr->pkt_size;
140}
141
142int gpr_send_pkt(struct apr_device *gdev, struct gpr_pkt *pkt)
143{
144 return pkt_router_send_svc_pkt(&gdev->svc, pkt);
145}
146EXPORT_SYMBOL_GPL(gpr_send_pkt);
147
148int gpr_send_port_pkt(gpr_port_t *port, struct gpr_pkt *pkt)
149{
150 return pkt_router_send_svc_pkt(port, pkt);
151}
152EXPORT_SYMBOL_GPL(gpr_send_port_pkt);
153
154static void apr_dev_release(struct device *dev)
155{
156 struct apr_device *adev = to_apr_device(dev);
157
158 kfree(adev);
159}
160
161static int apr_callback(struct rpmsg_device *rpdev, void *buf,
162 int len, void *priv, u32 addr)
163{
164 struct packet_router *apr = dev_get_drvdata(&rpdev->dev);
165 struct apr_rx_buf *abuf;
166 unsigned long flags;
167
168 if (len <= APR_HDR_SIZE) {
169 dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n",
170 buf, len);
171 return -EINVAL;
172 }
173
174 abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC);
175 if (!abuf)
176 return -ENOMEM;
177
178 abuf->len = len;
179 memcpy(abuf->buf, buf, len);
180
181 spin_lock_irqsave(&apr->rx_lock, flags);
182 list_add_tail(&abuf->node, &apr->rx_list);
183 spin_unlock_irqrestore(&apr->rx_lock, flags);
184
185 queue_work(apr->rxwq, &apr->rx_work);
186
187 return 0;
188}
189
190static int apr_do_rx_callback(struct packet_router *apr, struct apr_rx_buf *abuf)
191{
192 uint16_t hdr_size, msg_type, ver, svc_id;
193 struct pkt_router_svc *svc;
194 struct apr_device *adev;
195 struct apr_driver *adrv = NULL;
196 struct apr_resp_pkt resp;
197 struct apr_hdr *hdr;
198 unsigned long flags;
199 void *buf = abuf->buf;
200 int len = abuf->len;
201
202 hdr = buf;
203 ver = APR_HDR_FIELD_VER(hdr->hdr_field);
204 if (ver > APR_PKT_VER + 1)
205 return -EINVAL;
206
207 hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field);
208 if (hdr_size < APR_HDR_SIZE) {
209 dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size);
210 return -EINVAL;
211 }
212
213 if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {
214 dev_err(apr->dev, "APR: Wrong packet size\n");
215 return -EINVAL;
216 }
217
218 msg_type = APR_HDR_FIELD_MT(hdr->hdr_field);
219 if (msg_type >= APR_MSG_TYPE_MAX) {
220 dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type);
221 return -EINVAL;
222 }
223
224 if (hdr->src_domain >= APR_DOMAIN_MAX ||
225 hdr->dest_domain >= APR_DOMAIN_MAX ||
226 hdr->src_svc >= APR_SVC_MAX ||
227 hdr->dest_svc >= APR_SVC_MAX) {
228 dev_err(apr->dev, "APR: Wrong APR header\n");
229 return -EINVAL;
230 }
231
232 svc_id = hdr->dest_svc;
233 spin_lock_irqsave(&apr->svcs_lock, flags);
234 svc = idr_find(&apr->svcs_idr, svc_id);
235 if (svc && svc->dev->driver) {
236 adev = svc_to_apr_device(svc);
237 adrv = to_apr_driver(adev->dev.driver);
238 }
239 spin_unlock_irqrestore(&apr->svcs_lock, flags);
240
241 if (!adrv || !adev) {
242 dev_err(apr->dev, "APR: service is not registered (%d)\n",
243 svc_id);
244 return -EINVAL;
245 }
246
247 resp.hdr = *hdr;
248 resp.payload_size = hdr->pkt_size - hdr_size;
249
250 /*
251 * NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include
252 * optional headers in to apr_hdr which should be ignored
253 */
254 if (resp.payload_size > 0)
255 resp.payload = buf + hdr_size;
256
257 adrv->callback(adev, &resp);
258
259 return 0;
260}
261
262static int gpr_do_rx_callback(struct packet_router *gpr, struct apr_rx_buf *abuf)
263{
264 uint16_t hdr_size, ver;
265 struct pkt_router_svc *svc = NULL;
266 struct gpr_resp_pkt resp;
267 struct gpr_hdr *hdr;
268 unsigned long flags;
269 void *buf = abuf->buf;
270 int len = abuf->len;
271
272 hdr = buf;
273 ver = hdr->version;
274 if (ver > GPR_PKT_VER + 1)
275 return -EINVAL;
276
277 hdr_size = hdr->hdr_size;
278 if (hdr_size < GPR_PKT_HEADER_WORD_SIZE) {
279 dev_err(gpr->dev, "GPR: Wrong hdr size:%d\n", hdr_size);
280 return -EINVAL;
281 }
282
283 if (hdr->pkt_size < GPR_PKT_HEADER_BYTE_SIZE || hdr->pkt_size != len) {
284 dev_err(gpr->dev, "GPR: Wrong packet size\n");
285 return -EINVAL;
286 }
287
288 resp.hdr = *hdr;
289 resp.payload_size = hdr->pkt_size - (hdr_size * 4);
290
291 /*
292 * NOTE: hdr_size is not same as GPR_HDR_SIZE as remote can include
293 * optional headers in to gpr_hdr which should be ignored
294 */
295 if (resp.payload_size > 0)
296 resp.payload = buf + (hdr_size * 4);
297
298
299 spin_lock_irqsave(&gpr->svcs_lock, flags);
300 svc = idr_find(&gpr->svcs_idr, hdr->dest_port);
301 spin_unlock_irqrestore(&gpr->svcs_lock, flags);
302
303 if (!svc) {
304 dev_err(gpr->dev, "GPR: Port(%x) is not registered\n",
305 hdr->dest_port);
306 return -EINVAL;
307 }
308
309 if (svc->callback)
310 svc->callback(&resp, svc->priv, 0);
311
312 return 0;
313}
314
315static void apr_rxwq(struct work_struct *work)
316{
317 struct packet_router *apr = container_of(work, struct packet_router, rx_work);
318 struct apr_rx_buf *abuf, *b;
319 unsigned long flags;
320
321 if (!list_empty(&apr->rx_list)) {
322 list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {
323 switch (apr->type) {
324 case PR_TYPE_APR:
325 apr_do_rx_callback(apr, abuf);
326 break;
327 case PR_TYPE_GPR:
328 gpr_do_rx_callback(apr, abuf);
329 break;
330 default:
331 break;
332 }
333 spin_lock_irqsave(&apr->rx_lock, flags);
334 list_del(&abuf->node);
335 spin_unlock_irqrestore(&apr->rx_lock, flags);
336 kfree(abuf);
337 }
338 }
339}
340
341static int apr_device_match(struct device *dev, struct device_driver *drv)
342{
343 struct apr_device *adev = to_apr_device(dev);
344 struct apr_driver *adrv = to_apr_driver(drv);
345 const struct apr_device_id *id = adrv->id_table;
346
347 /* Attempt an OF style match first */
348 if (of_driver_match_device(dev, drv))
349 return 1;
350
351 if (!id)
352 return 0;
353
354 while (id->domain_id != 0 || id->svc_id != 0) {
355 if (id->domain_id == adev->domain_id &&
356 id->svc_id == adev->svc.id)
357 return 1;
358 id++;
359 }
360
361 return 0;
362}
363
364static int apr_device_probe(struct device *dev)
365{
366 struct apr_device *adev = to_apr_device(dev);
367 struct apr_driver *adrv = to_apr_driver(dev->driver);
368 int ret;
369
370 ret = adrv->probe(adev);
371 if (!ret)
372 adev->svc.callback = adrv->gpr_callback;
373
374 return ret;
375}
376
377static void apr_device_remove(struct device *dev)
378{
379 struct apr_device *adev = to_apr_device(dev);
380 struct apr_driver *adrv = to_apr_driver(dev->driver);
381 struct packet_router *apr = dev_get_drvdata(adev->dev.parent);
382
383 if (adrv->remove)
384 adrv->remove(adev);
385 spin_lock(&apr->svcs_lock);
386 idr_remove(&apr->svcs_idr, adev->svc.id);
387 spin_unlock(&apr->svcs_lock);
388}
389
390static int apr_uevent(struct device *dev, struct kobj_uevent_env *env)
391{
392 struct apr_device *adev = to_apr_device(dev);
393 int ret;
394
395 ret = of_device_uevent_modalias(dev, env);
396 if (ret != -ENODEV)
397 return ret;
398
399 return add_uevent_var(env, "MODALIAS=apr:%s", adev->name);
400}
401
402struct bus_type aprbus = {
403 .name = "aprbus",
404 .match = apr_device_match,
405 .probe = apr_device_probe,
406 .uevent = apr_uevent,
407 .remove = apr_device_remove,
408};
409EXPORT_SYMBOL_GPL(aprbus);
410
411static int apr_add_device(struct device *dev, struct device_node *np,
412 u32 svc_id, u32 domain_id)
413{
414 struct packet_router *apr = dev_get_drvdata(dev);
415 struct apr_device *adev = NULL;
416 struct pkt_router_svc *svc;
417 int ret;
418
419 adev = kzalloc(sizeof(*adev), GFP_KERNEL);
420 if (!adev)
421 return -ENOMEM;
422
423 adev->svc_id = svc_id;
424 svc = &adev->svc;
425
426 svc->id = svc_id;
427 svc->pr = apr;
428 svc->priv = adev;
429 svc->dev = dev;
430 spin_lock_init(&svc->lock);
431
432 adev->domain_id = domain_id;
433
434 if (np)
435 snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
436
437 switch (apr->type) {
438 case PR_TYPE_APR:
439 dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
440 domain_id, svc_id);
441 break;
442 case PR_TYPE_GPR:
443 dev_set_name(&adev->dev, "gprsvc:%s:%x:%x", adev->name,
444 domain_id, svc_id);
445 break;
446 default:
447 break;
448 }
449
450 adev->dev.bus = &aprbus;
451 adev->dev.parent = dev;
452 adev->dev.of_node = np;
453 adev->dev.release = apr_dev_release;
454 adev->dev.driver = NULL;
455
456 spin_lock(&apr->svcs_lock);
457 ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC);
458 spin_unlock(&apr->svcs_lock);
459 if (ret < 0) {
460 dev_err(dev, "idr_alloc failed: %d\n", ret);
461 goto out;
462 }
463
464 /* Protection domain is optional, it does not exist on older platforms */
465 ret = of_property_read_string_index(np, "qcom,protection-domain",
466 1, &adev->service_path);
467 if (ret < 0 && ret != -EINVAL) {
468 dev_err(dev, "Failed to read second value of qcom,protection-domain\n");
469 goto out;
470 }
471
472 dev_info(dev, "Adding APR/GPR dev: %s\n", dev_name(&adev->dev));
473
474 ret = device_register(&adev->dev);
475 if (ret) {
476 dev_err(dev, "device_register failed: %d\n", ret);
477 put_device(&adev->dev);
478 }
479
480out:
481 return ret;
482}
483
484static int of_apr_add_pd_lookups(struct device *dev)
485{
486 const char *service_name, *service_path;
487 struct packet_router *apr = dev_get_drvdata(dev);
488 struct device_node *node;
489 struct pdr_service *pds;
490 int ret;
491
492 for_each_child_of_node(dev->of_node, node) {
493 ret = of_property_read_string_index(node, "qcom,protection-domain",
494 0, &service_name);
495 if (ret < 0)
496 continue;
497
498 ret = of_property_read_string_index(node, "qcom,protection-domain",
499 1, &service_path);
500 if (ret < 0) {
501 dev_err(dev, "pdr service path missing: %d\n", ret);
502 of_node_put(node);
503 return ret;
504 }
505
506 pds = pdr_add_lookup(apr->pdr, service_name, service_path);
507 if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
508 dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));
509 of_node_put(node);
510 return PTR_ERR(pds);
511 }
512 }
513
514 return 0;
515}
516
517static void of_register_apr_devices(struct device *dev, const char *svc_path)
518{
519 struct packet_router *apr = dev_get_drvdata(dev);
520 struct device_node *node;
521 const char *service_path;
522 int ret;
523
524 for_each_child_of_node(dev->of_node, node) {
525 u32 svc_id;
526 u32 domain_id;
527
528 /*
529 * This function is called with svc_path NULL during
530 * apr_probe(), in which case we register any apr devices
531 * without a qcom,protection-domain specified.
532 *
533 * Then as the protection domains becomes available
534 * (if applicable) this function is again called, but with
535 * svc_path representing the service becoming available. In
536 * this case we register any apr devices with a matching
537 * qcom,protection-domain.
538 */
539
540 ret = of_property_read_string_index(node, "qcom,protection-domain",
541 1, &service_path);
542 if (svc_path) {
543 /* skip APR services that are PD independent */
544 if (ret)
545 continue;
546
547 /* skip APR services whose PD paths don't match */
548 if (strcmp(service_path, svc_path))
549 continue;
550 } else {
551 /* skip APR services whose PD lookups are registered */
552 if (ret == 0)
553 continue;
554 }
555
556 if (of_property_read_u32(node, "reg", &svc_id))
557 continue;
558
559 domain_id = apr->dest_domain_id;
560
561 if (apr_add_device(dev, node, svc_id, domain_id))
562 dev_err(dev, "Failed to add apr %d svc\n", svc_id);
563 }
564}
565
566static int apr_remove_device(struct device *dev, void *svc_path)
567{
568 struct apr_device *adev = to_apr_device(dev);
569
570 if (svc_path && adev->service_path) {
571 if (!strcmp(adev->service_path, (char *)svc_path))
572 device_unregister(&adev->dev);
573 } else {
574 device_unregister(&adev->dev);
575 }
576
577 return 0;
578}
579
580static void apr_pd_status(int state, char *svc_path, void *priv)
581{
582 struct packet_router *apr = (struct packet_router *)priv;
583
584 switch (state) {
585 case SERVREG_SERVICE_STATE_UP:
586 of_register_apr_devices(apr->dev, svc_path);
587 break;
588 case SERVREG_SERVICE_STATE_DOWN:
589 device_for_each_child(apr->dev, svc_path, apr_remove_device);
590 break;
591 }
592}
593
594static int apr_probe(struct rpmsg_device *rpdev)
595{
596 struct device *dev = &rpdev->dev;
597 struct packet_router *apr;
598 int ret;
599
600 apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
601 if (!apr)
602 return -ENOMEM;
603
604 ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id);
605
606 if (of_device_is_compatible(dev->of_node, "qcom,gpr")) {
607 apr->type = PR_TYPE_GPR;
608 } else {
609 if (ret) /* try deprecated apr-domain property */
610 ret = of_property_read_u32(dev->of_node, "qcom,apr-domain",
611 &apr->dest_domain_id);
612 apr->type = PR_TYPE_APR;
613 }
614
615 if (ret) {
616 dev_err(dev, "Domain ID not specified in DT\n");
617 return ret;
618 }
619
620 dev_set_drvdata(dev, apr);
621 apr->ch = rpdev->ept;
622 apr->dev = dev;
623 apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
624 if (!apr->rxwq) {
625 dev_err(apr->dev, "Failed to start Rx WQ\n");
626 return -ENOMEM;
627 }
628 INIT_WORK(&apr->rx_work, apr_rxwq);
629
630 apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
631 if (IS_ERR(apr->pdr)) {
632 dev_err(dev, "Failed to init PDR handle\n");
633 ret = PTR_ERR(apr->pdr);
634 goto destroy_wq;
635 }
636
637 INIT_LIST_HEAD(&apr->rx_list);
638 spin_lock_init(&apr->rx_lock);
639 spin_lock_init(&apr->svcs_lock);
640 idr_init(&apr->svcs_idr);
641
642 ret = of_apr_add_pd_lookups(dev);
643 if (ret)
644 goto handle_release;
645
646 of_register_apr_devices(dev, NULL);
647
648 return 0;
649
650handle_release:
651 pdr_handle_release(apr->pdr);
652destroy_wq:
653 destroy_workqueue(apr->rxwq);
654 return ret;
655}
656
657static void apr_remove(struct rpmsg_device *rpdev)
658{
659 struct packet_router *apr = dev_get_drvdata(&rpdev->dev);
660
661 pdr_handle_release(apr->pdr);
662 device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
663 destroy_workqueue(apr->rxwq);
664}
665
666/*
667 * __apr_driver_register() - Client driver registration with aprbus
668 *
669 * @drv:Client driver to be associated with client-device.
670 * @owner: owning module/driver
671 *
672 * This API will register the client driver with the aprbus
673 * It is called from the driver's module-init function.
674 */
675int __apr_driver_register(struct apr_driver *drv, struct module *owner)
676{
677 drv->driver.bus = &aprbus;
678 drv->driver.owner = owner;
679
680 return driver_register(&drv->driver);
681}
682EXPORT_SYMBOL_GPL(__apr_driver_register);
683
684/*
685 * apr_driver_unregister() - Undo effect of apr_driver_register
686 *
687 * @drv: Client driver to be unregistered
688 */
689void apr_driver_unregister(struct apr_driver *drv)
690{
691 driver_unregister(&drv->driver);
692}
693EXPORT_SYMBOL_GPL(apr_driver_unregister);
694
695static const struct of_device_id pkt_router_of_match[] = {
696 { .compatible = "qcom,apr"},
697 { .compatible = "qcom,apr-v2"},
698 { .compatible = "qcom,gpr"},
699 {}
700};
701MODULE_DEVICE_TABLE(of, pkt_router_of_match);
702
703static struct rpmsg_driver packet_router_driver = {
704 .probe = apr_probe,
705 .remove = apr_remove,
706 .callback = apr_callback,
707 .drv = {
708 .name = "qcom,apr",
709 .of_match_table = pkt_router_of_match,
710 },
711};
712
713static int __init apr_init(void)
714{
715 int ret;
716
717 ret = bus_register(&aprbus);
718 if (!ret)
719 ret = register_rpmsg_driver(&packet_router_driver);
720 else
721 bus_unregister(&aprbus);
722
723 return ret;
724}
725
726static void __exit apr_exit(void)
727{
728 bus_unregister(&aprbus);
729 unregister_rpmsg_driver(&packet_router_driver);
730}
731
732subsys_initcall(apr_init);
733module_exit(apr_exit);
734
735MODULE_LICENSE("GPL v2");
736MODULE_DESCRIPTION("Qualcomm APR Bus");