forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-05-10 23fa18eaa71266feff7ba8d83022d9e1cc83c65a 
 kernel/drivers/soc/qcom/apr.c
....@@ -8,17 +8,35 @@
88 #include <linux/spinlock.h>
99 #include <linux/idr.h>
1010 #include <linux/slab.h>
11
+#include <linux/workqueue.h>
1112 #include <linux/of_device.h>
1213 #include <linux/soc/qcom/apr.h>
14
+#include <linux/soc/qcom/pdr.h>
1315 #include <linux/rpmsg.h>
1416 #include <linux/of.h>
1517 
16
-struct apr {
18
+enum {
19
+	PR_TYPE_APR = 0,
20
+};
21
+
22
+struct packet_router {
1723 	struct rpmsg_endpoint *ch;
1824 	struct device *dev;
1925 	spinlock_t svcs_lock;
26
+	spinlock_t rx_lock;
2027 	struct idr svcs_idr;
2128 	int dest_domain_id;
29
+	int type;
30
+	struct pdr_handle *pdr;
31
+	struct workqueue_struct *rxwq;
32
+	struct work_struct rx_work;
33
+	struct list_head rx_list;
34
+};
35
+
36
+struct apr_rx_buf {
37
+	struct list_head node;
38
+	int len;
39
+	uint8_t buf[];
2240 };
2341 
2442 /**
....@@ -31,21 +49,21 @@
3149  */
3250 int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt)
3351 {
34
-	struct apr *apr = dev_get_drvdata(adev->dev.parent);
52
+	struct packet_router *apr = dev_get_drvdata(adev->dev.parent);
3553 	struct apr_hdr *hdr;
3654 	unsigned long flags;
3755 	int ret;
3856 
39
-	spin_lock_irqsave(&adev->lock, flags);
57
+	spin_lock_irqsave(&adev->svc.lock, flags);
4058 
4159 	hdr = &pkt->hdr;
4260 	hdr->src_domain = APR_DOMAIN_APPS;
43
-	hdr->src_svc = adev->svc_id;
61
+	hdr->src_svc = adev->svc.id;
4462 	hdr->dest_domain = adev->domain_id;
45
-	hdr->dest_svc = adev->svc_id;
63
+	hdr->dest_svc = adev->svc.id;
4664 
4765 	ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);
48
-	spin_unlock_irqrestore(&adev->lock, flags);
66
+	spin_unlock_irqrestore(&adev->svc.lock, flags);
4967 
5068 	return ret ? ret : hdr->pkt_size;
5169 }
....@@ -61,12 +79,8 @@
6179 static int apr_callback(struct rpmsg_device *rpdev, void *buf,
6280 				  int len, void *priv, u32 addr)
6381 {
64
-	struct apr *apr = dev_get_drvdata(&rpdev->dev);
65
-	uint16_t hdr_size, msg_type, ver, svc_id;
66
-	struct apr_device *svc = NULL;
67
-	struct apr_driver *adrv = NULL;
68
-	struct apr_resp_pkt resp;
69
-	struct apr_hdr *hdr;
82
+	struct packet_router *apr = dev_get_drvdata(&rpdev->dev);
83
+	struct apr_rx_buf *abuf;
7084 	unsigned long flags;
7185 
7286 	if (len <= APR_HDR_SIZE) {
....@@ -74,6 +88,34 @@
7488 			buf, len);
7589 		return -EINVAL;
7690 	}
91
+
92
+	abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC);
93
+	if (!abuf)
94
+		return -ENOMEM;
95
+
96
+	abuf->len = len;
97
+	memcpy(abuf->buf, buf, len);
98
+
99
+	spin_lock_irqsave(&apr->rx_lock, flags);
100
+	list_add_tail(&abuf->node, &apr->rx_list);
101
+	spin_unlock_irqrestore(&apr->rx_lock, flags);
102
+
103
+	queue_work(apr->rxwq, &apr->rx_work);
104
+
105
+	return 0;
106
+}
107
+
108
+static int apr_do_rx_callback(struct packet_router *apr, struct apr_rx_buf *abuf)
109
+{
110
+	uint16_t hdr_size, msg_type, ver, svc_id;
111
+	struct pkt_router_svc *svc;
112
+	struct apr_device *adev;
113
+	struct apr_driver *adrv = NULL;
114
+	struct apr_resp_pkt resp;
115
+	struct apr_hdr *hdr;
116
+	unsigned long flags;
117
+	void *buf = abuf->buf;
118
+	int len = abuf->len;
77119 
78120 	hdr = buf;
79121 	ver = APR_HDR_FIELD_VER(hdr->hdr_field);
....@@ -87,7 +129,7 @@
87129 	}
88130 
89131 	if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {
90
-		dev_err(apr->dev, "APR: Wrong paket size\n");
132
+		dev_err(apr->dev, "APR: Wrong packet size\n");
91133 		return -EINVAL;
92134 	}
93135 
....@@ -108,12 +150,15 @@
108150 	svc_id = hdr->dest_svc;
109151 	spin_lock_irqsave(&apr->svcs_lock, flags);
110152 	svc = idr_find(&apr->svcs_idr, svc_id);
111
-	if (svc && svc->dev.driver)
112
-		adrv = to_apr_driver(svc->dev.driver);
153
+	if (svc && svc->dev->driver) {
154
+		adev = svc_to_apr_device(svc);
155
+		adrv = to_apr_driver(adev->dev.driver);
156
+	}
113157 	spin_unlock_irqrestore(&apr->svcs_lock, flags);
114158 
115
-	if (!adrv) {
116
-		dev_err(apr->dev, "APR: service is not registered\n");
159
+	if (!adrv || !adev) {
160
+		dev_err(apr->dev, "APR: service is not registered (%d)\n",
161
+			svc_id);
117162 		return -EINVAL;
118163 	}
119164 
....@@ -127,9 +172,32 @@
127172 	if (resp.payload_size > 0)
128173 		resp.payload = buf + hdr_size;
129174 
130
-	adrv->callback(svc, &resp);
175
+	adrv->callback(adev, &resp);
131176 
132177 	return 0;
178
+}
179
+
180
+static void apr_rxwq(struct work_struct *work)
181
+{
182
+	struct packet_router *apr = container_of(work, struct packet_router, rx_work);
183
+	struct apr_rx_buf *abuf, *b;
184
+	unsigned long flags;
185
+
186
+	if (!list_empty(&apr->rx_list)) {
187
+		list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {
188
+			switch (apr->type) {
189
+			case PR_TYPE_APR:
190
+				apr_do_rx_callback(apr, abuf);
191
+				break;
192
+			default:
193
+				break;
194
+			}
195
+			spin_lock_irqsave(&apr->rx_lock, flags);
196
+			list_del(&abuf->node);
197
+			spin_unlock_irqrestore(&apr->rx_lock, flags);
198
+			kfree(abuf);
199
+		}
200
+	}
133201 }
134202 
135203 static int apr_device_match(struct device *dev, struct device_driver *drv)
....@@ -147,7 +215,7 @@
147215 
148216 	while (id->domain_id != 0 || id->svc_id != 0) {
149217 		if (id->domain_id == adev->domain_id &&
150
-		    id->svc_id == adev->svc_id)
218
+		    id->svc_id == adev->svc.id)
151219 			return 1;
152220 		id++;
153221 	}
....@@ -167,14 +235,14 @@
167235 {
168236 	struct apr_device *adev = to_apr_device(dev);
169237 	struct apr_driver *adrv;
170
-	struct apr *apr = dev_get_drvdata(adev->dev.parent);
238
+	struct packet_router *apr = dev_get_drvdata(adev->dev.parent);
171239 
172240 	if (dev->driver) {
173241 		adrv = to_apr_driver(dev->driver);
174242 		if (adrv->remove)
175243 			adrv->remove(adev);
176244 		spin_lock(&apr->svcs_lock);
177
-		idr_remove(&apr->svcs_idr, adev->svc_id);
245
+		idr_remove(&apr->svcs_idr, adev->svc.id);
178246 		spin_unlock(&apr->svcs_lock);
179247 	}
180248 
....@@ -203,28 +271,39 @@
203271 EXPORT_SYMBOL_GPL(aprbus);
204272 
205273 static int apr_add_device(struct device *dev, struct device_node *np,
206
-			  const struct apr_device_id *id)
274
+			  u32 svc_id, u32 domain_id)
207275 {
208
-	struct apr *apr = dev_get_drvdata(dev);
276
+	struct packet_router *apr = dev_get_drvdata(dev);
209277 	struct apr_device *adev = NULL;
278
+	struct pkt_router_svc *svc;
210279 	int ret;
211280 
212281 	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
213282 	if (!adev)
214283 		return -ENOMEM;
215284 
216
-	spin_lock_init(&adev->lock);
285
+	adev->svc_id = svc_id;
286
+	svc = &adev->svc;
217287 
218
-	adev->svc_id = id->svc_id;
219
-	adev->domain_id = id->domain_id;
220
-	adev->version = id->svc_version;
288
+	svc->id = svc_id;
289
+	svc->pr = apr;
290
+	svc->priv = adev;
291
+	svc->dev = dev;
292
+	spin_lock_init(&svc->lock);
293
+
294
+	adev->domain_id = domain_id;
295
+
221296 	if (np)
222
-		strscpy(adev->name, np->name, APR_NAME_SIZE);
223
-	else
224
-		strscpy(adev->name, id->name, APR_NAME_SIZE);
297
+		snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
225298 
226
-	dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
227
-		     id->domain_id, id->svc_id);
299
+	switch (apr->type) {
300
+	case PR_TYPE_APR:
301
+		dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
302
+			     domain_id, svc_id);
303
+		break;
304
+	default:
305
+		break;
306
+	}
228307 
229308 	adev->dev.bus = &aprbus;
230309 	adev->dev.parent = dev;
....@@ -233,9 +312,20 @@
233312 	adev->dev.driver = NULL;
234313 
235314 	spin_lock(&apr->svcs_lock);
236
-	idr_alloc(&apr->svcs_idr, adev, id->svc_id,
237
-		  id->svc_id + 1, GFP_ATOMIC);
315
+	ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC);
238316 	spin_unlock(&apr->svcs_lock);
317
+	if (ret < 0) {
318
+		dev_err(dev, "idr_alloc failed: %d\n", ret);
319
+		goto out;
320
+	}
321
+
322
+	/* Protection domain is optional, it does not exist on older platforms */
323
+	ret = of_property_read_string_index(np, "qcom,protection-domain",
324
+					    1, &adev->service_path);
325
+	if (ret < 0 && ret != -EINVAL) {
326
+		dev_err(dev, "Failed to read second value of qcom,protection-domain\n");
327
+		goto out;
328
+	}
239329 
240330 	dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
241331 
....@@ -245,65 +335,185 @@
245335 		put_device(&adev->dev);
246336 	}
247337 
338
+out:
248339 	return ret;
249340 }
250341 
251
-static void of_register_apr_devices(struct device *dev)
342
+static int of_apr_add_pd_lookups(struct device *dev)
252343 {
253
-	struct apr *apr = dev_get_drvdata(dev);
344
+	const char *service_name, *service_path;
345
+	struct packet_router *apr = dev_get_drvdata(dev);
254346 	struct device_node *node;
347
+	struct pdr_service *pds;
348
+	int ret;
255349 
256350 	for_each_child_of_node(dev->of_node, node) {
257
-		struct apr_device_id id = { {0} };
258
-
259
-		if (of_property_read_u32(node, "reg", &id.svc_id))
351
+		ret = of_property_read_string_index(node, "qcom,protection-domain",
352
+						    0, &service_name);
353
+		if (ret < 0)
260354 			continue;
261355 
262
-		id.domain_id = apr->dest_domain_id;
356
+		ret = of_property_read_string_index(node, "qcom,protection-domain",
357
+						    1, &service_path);
358
+		if (ret < 0) {
359
+			dev_err(dev, "pdr service path missing: %d\n", ret);
360
+			of_node_put(node);
361
+			return ret;
362
+		}
263363 
264
-		if (apr_add_device(dev, node, &id))
265
-			dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
364
+		pds = pdr_add_lookup(apr->pdr, service_name, service_path);
365
+		if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
366
+			dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));
367
+			of_node_put(node);
368
+			return PTR_ERR(pds);
369
+		}
370
+	}
371
+
372
+	return 0;
373
+}
374
+
375
+static void of_register_apr_devices(struct device *dev, const char *svc_path)
376
+{
377
+	struct packet_router *apr = dev_get_drvdata(dev);
378
+	struct device_node *node;
379
+	const char *service_path;
380
+	int ret;
381
+
382
+	for_each_child_of_node(dev->of_node, node) {
383
+		u32 svc_id;
384
+		u32 domain_id;
385
+
386
+		/*
387
+		 * This function is called with svc_path NULL during
388
+		 * apr_probe(), in which case we register any apr devices
389
+		 * without a qcom,protection-domain specified.
390
+		 *
391
+		 * Then as the protection domains becomes available
392
+		 * (if applicable) this function is again called, but with
393
+		 * svc_path representing the service becoming available. In
394
+		 * this case we register any apr devices with a matching
395
+		 * qcom,protection-domain.
396
+		 */
397
+
398
+		ret = of_property_read_string_index(node, "qcom,protection-domain",
399
+						    1, &service_path);
400
+		if (svc_path) {
401
+			/* skip APR services that are PD independent */
402
+			if (ret)
403
+				continue;
404
+
405
+			/* skip APR services whose PD paths don't match */
406
+			if (strcmp(service_path, svc_path))
407
+				continue;
408
+		} else {
409
+			/* skip APR services whose PD lookups are registered */
410
+			if (ret == 0)
411
+				continue;
412
+		}
413
+
414
+		if (of_property_read_u32(node, "reg", &svc_id))
415
+			continue;
416
+
417
+		domain_id = apr->dest_domain_id;
418
+
419
+		if (apr_add_device(dev, node, svc_id, domain_id))
420
+			dev_err(dev, "Failed to add apr %d svc\n", svc_id);
421
+	}
422
+}
423
+
424
+static int apr_remove_device(struct device *dev, void *svc_path)
425
+{
426
+	struct apr_device *adev = to_apr_device(dev);
427
+
428
+	if (svc_path && adev->service_path) {
429
+		if (!strcmp(adev->service_path, (char *)svc_path))
430
+			device_unregister(&adev->dev);
431
+	} else {
432
+		device_unregister(&adev->dev);
433
+	}
434
+
435
+	return 0;
436
+}
437
+
438
+static void apr_pd_status(int state, char *svc_path, void *priv)
439
+{
440
+	struct packet_router *apr = (struct packet_router *)priv;
441
+
442
+	switch (state) {
443
+	case SERVREG_SERVICE_STATE_UP:
444
+		of_register_apr_devices(apr->dev, svc_path);
445
+		break;
446
+	case SERVREG_SERVICE_STATE_DOWN:
447
+		device_for_each_child(apr->dev, svc_path, apr_remove_device);
448
+		break;
266449 	}
267450 }
268451 
269452 static int apr_probe(struct rpmsg_device *rpdev)
270453 {
271454 	struct device *dev = &rpdev->dev;
272
-	struct apr *apr;
455
+	struct packet_router *apr;
273456 	int ret;
274457 
275458 	apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
276459 	if (!apr)
277460 		return -ENOMEM;
278461 
279
-	ret = of_property_read_u32(dev->of_node, "reg", &apr->dest_domain_id);
462
+	ret = of_property_read_u32(dev->of_node, "qcom,domain", &apr->dest_domain_id);
463
+	if (ret) /* try deprecated apr-domain property */
464
+		ret = of_property_read_u32(dev->of_node, "qcom,apr-domain",
465
+					   &apr->dest_domain_id);
466
+	apr->type = PR_TYPE_APR;
280467 	if (ret) {
281
-		dev_err(dev, "APR Domain ID not specified in DT\n");
468
+		dev_err(dev, "Domain ID not specified in DT\n");
282469 		return ret;
283470 	}
284471 
285472 	dev_set_drvdata(dev, apr);
286473 	apr->ch = rpdev->ept;
287474 	apr->dev = dev;
475
+	apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
476
+	if (!apr->rxwq) {
477
+		dev_err(apr->dev, "Failed to start Rx WQ\n");
478
+		return -ENOMEM;
479
+	}
480
+	INIT_WORK(&apr->rx_work, apr_rxwq);
481
+
482
+	apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
483
+	if (IS_ERR(apr->pdr)) {
484
+		dev_err(dev, "Failed to init PDR handle\n");
485
+		ret = PTR_ERR(apr->pdr);
486
+		goto destroy_wq;
487
+	}
488
+
489
+	INIT_LIST_HEAD(&apr->rx_list);
490
+	spin_lock_init(&apr->rx_lock);
288491 	spin_lock_init(&apr->svcs_lock);
289492 	idr_init(&apr->svcs_idr);
290
-	of_register_apr_devices(dev);
493
+
494
+	ret = of_apr_add_pd_lookups(dev);
495
+	if (ret)
496
+		goto handle_release;
497
+
498
+	of_register_apr_devices(dev, NULL);
291499 
292500 	return 0;
293
-}
294501 
295
-static int apr_remove_device(struct device *dev, void *null)
296
-{
297
-	struct apr_device *adev = to_apr_device(dev);
298
-
299
-	device_unregister(&adev->dev);
300
-
301
-	return 0;
502
+handle_release:
503
+	pdr_handle_release(apr->pdr);
504
+destroy_wq:
505
+	destroy_workqueue(apr->rxwq);
506
+	return ret;
302507 }
303508 
304509 static void apr_remove(struct rpmsg_device *rpdev)
305510 {
511
+	struct packet_router *apr = dev_get_drvdata(&rpdev->dev);
512
+
513
+	pdr_handle_release(apr->pdr);
306514 	device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
515
+	flush_workqueue(apr->rxwq);
516
+	destroy_workqueue(apr->rxwq);
307517 }
308518 
309519 /*
....@@ -335,20 +545,20 @@
335545 }
336546 EXPORT_SYMBOL_GPL(apr_driver_unregister);
337547 
338
-static const struct of_device_id apr_of_match[] = {
548
+static const struct of_device_id pkt_router_of_match[] = {
339549 	{ .compatible = "qcom,apr"},
340550 	{ .compatible = "qcom,apr-v2"},
341551 	{}
342552 };
343
-MODULE_DEVICE_TABLE(of, apr_of_match);
553
+MODULE_DEVICE_TABLE(of, pkt_router_of_match);
344554 
345
-static struct rpmsg_driver apr_driver = {
555
+static struct rpmsg_driver packet_router_driver = {
346556 	.probe = apr_probe,
347557 	.remove = apr_remove,
348558 	.callback = apr_callback,
349559 	.drv = {
350560 		.name = "qcom,apr",
351
-		.of_match_table = apr_of_match,
561
+		.of_match_table = pkt_router_of_match,
352562 	},
353563 };
354564 
....@@ -358,7 +568,7 @@
358568 
359569 	ret = bus_register(&aprbus);
360570 	if (!ret)
361
-		ret = register_rpmsg_driver(&apr_driver);
571
+		ret = register_rpmsg_driver(&packet_router_driver);
362572 	else
363573 		bus_unregister(&aprbus);
364574 
....@@ -368,7 +578,7 @@
368578 static void __exit apr_exit(void)
369579 {
370580 	bus_unregister(&aprbus);
371
-	unregister_rpmsg_driver(&apr_driver);
581
+	unregister_rpmsg_driver(&packet_router_driver);
372582 }
373583 
374584 subsys_initcall(apr_init);