add new system file

hc

2024-02-20 102a0743326a03cd1a1202ceda21e175b7d3575c

 kernel/drivers/soc/qcom/apr.c
..
..
@@ -15,13 +15,18 @@
15
15
 #include <linux/rpmsg.h>
16
16
 #include <linux/of.h>
17
17
 
18
-struct apr {
18
+enum {
19
+	PR_TYPE_APR = 0,
20
+};
21
+
22
+struct packet_router {
19
23
 	struct rpmsg_endpoint *ch;
20
24
 	struct device *dev;
21
25
 	spinlock_t svcs_lock;
22
26
 	spinlock_t rx_lock;
23
27
 	struct idr svcs_idr;
24
28
 	int dest_domain_id;
29
+	int type;
25
30
 	struct pdr_handle *pdr;
26
31
 	struct workqueue_struct *rxwq;
27
32
 	struct work_struct rx_work;
..
..
@@ -44,21 +49,21 @@
44
49
  */
45
50
 int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt)
46
51
 {
47
-	struct apr *apr = dev_get_drvdata(adev->dev.parent);
52
+	struct packet_router *apr = dev_get_drvdata(adev->dev.parent);
48
53
 	struct apr_hdr *hdr;
49
54
 	unsigned long flags;
50
55
 	int ret;
51
56
 
52
-	spin_lock_irqsave(&adev->lock, flags);
57
+	spin_lock_irqsave(&adev->svc.lock, flags);
53
58
 
54
59
 	hdr = &pkt->hdr;
55
60
 	hdr->src_domain = APR_DOMAIN_APPS;
56
-	hdr->src_svc = adev->svc_id;
61
+	hdr->src_svc = adev->svc.id;
57
62
 	hdr->dest_domain = adev->domain_id;
58
-	hdr->dest_svc = adev->svc_id;
63
+	hdr->dest_svc = adev->svc.id;
59
64
 
60
65
 	ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);
61
-	spin_unlock_irqrestore(&adev->lock, flags);
66
+	spin_unlock_irqrestore(&adev->svc.lock, flags);
62
67
 
63
68
 	return ret ? ret : hdr->pkt_size;
64
69
 }
..
..
@@ -74,7 +79,7 @@
74
79
 static int apr_callback(struct rpmsg_device *rpdev, void *buf,
75
80
 				  int len, void *priv, u32 addr)
76
81
 {
77
-	struct apr *apr = dev_get_drvdata(&rpdev->dev);
82
+	struct packet_router *apr = dev_get_drvdata(&rpdev->dev);
78
83
 	struct apr_rx_buf *abuf;
79
84
 	unsigned long flags;
80
85
 
..
..
@@ -100,11 +105,11 @@
100
105
 	return 0;
101
106
 }
102
107
 
103
-
104
-static int apr_do_rx_callback(struct apr *apr, struct apr_rx_buf *abuf)
108
+static int apr_do_rx_callback(struct packet_router *apr, struct apr_rx_buf *abuf)
105
109
 {
106
110
 	uint16_t hdr_size, msg_type, ver, svc_id;
107
-	struct apr_device *svc = NULL;
111
+	struct pkt_router_svc *svc;
112
+	struct apr_device *adev;
108
113
 	struct apr_driver *adrv = NULL;
109
114
 	struct apr_resp_pkt resp;
110
115
 	struct apr_hdr *hdr;
..
..
@@ -145,12 +150,15 @@
145
150
 	svc_id = hdr->dest_svc;
146
151
 	spin_lock_irqsave(&apr->svcs_lock, flags);
147
152
 	svc = idr_find(&apr->svcs_idr, svc_id);
148
-	if (svc && svc->dev.driver)
149
-		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
+	}
150
157
 	spin_unlock_irqrestore(&apr->svcs_lock, flags);
151
158
 
152
-	if (!adrv) {
153
-		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);
154
162
 		return -EINVAL;
155
163
 	}
156
164
 
..
..
@@ -164,20 +172,26 @@
164
172
 	if (resp.payload_size > 0)
165
173
 		resp.payload = buf + hdr_size;
166
174
 
167
-	adrv->callback(svc, &resp);
175
+	adrv->callback(adev, &resp);
168
176
 
169
177
 	return 0;
170
178
 }
171
179
 
172
180
 static void apr_rxwq(struct work_struct *work)
173
181
 {
174
-	struct apr *apr = container_of(work, struct apr, rx_work);
182
+	struct packet_router *apr = container_of(work, struct packet_router, rx_work);
175
183
 	struct apr_rx_buf *abuf, *b;
176
184
 	unsigned long flags;
177
185
 
178
186
 	if (!list_empty(&apr->rx_list)) {
179
187
 		list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {
180
-			apr_do_rx_callback(apr, abuf);
188
+			switch (apr->type) {
189
+			case PR_TYPE_APR:
190
+				apr_do_rx_callback(apr, abuf);
191
+				break;
192
+			default:
193
+				break;
194
+			}
181
195
 			spin_lock_irqsave(&apr->rx_lock, flags);
182
196
 			list_del(&abuf->node);
183
197
 			spin_unlock_irqrestore(&apr->rx_lock, flags);
..
..
@@ -201,7 +215,7 @@
201
215
 
202
216
 	while (id->domain_id != 0 || id->svc_id != 0) {
203
217
 		if (id->domain_id == adev->domain_id &&
204
-		    id->svc_id == adev->svc_id)
218
+		    id->svc_id == adev->svc.id)
205
219
 			return 1;
206
220
 		id++;
207
221
 	}
..
..
@@ -221,14 +235,14 @@
221
235
 {
222
236
 	struct apr_device *adev = to_apr_device(dev);
223
237
 	struct apr_driver *adrv;
224
-	struct apr *apr = dev_get_drvdata(adev->dev.parent);
238
+	struct packet_router *apr = dev_get_drvdata(adev->dev.parent);
225
239
 
226
240
 	if (dev->driver) {
227
241
 		adrv = to_apr_driver(dev->driver);
228
242
 		if (adrv->remove)
229
243
 			adrv->remove(adev);
230
244
 		spin_lock(&apr->svcs_lock);
231
-		idr_remove(&apr->svcs_idr, adev->svc_id);
245
+		idr_remove(&apr->svcs_idr, adev->svc.id);
232
246
 		spin_unlock(&apr->svcs_lock);
233
247
 	}
234
248
 
..
..
@@ -257,28 +271,39 @@
257
271
 EXPORT_SYMBOL_GPL(aprbus);
258
272
 
259
273
 static int apr_add_device(struct device *dev, struct device_node *np,
260
-			  const struct apr_device_id *id)
274
+			  u32 svc_id, u32 domain_id)
261
275
 {
262
-	struct apr *apr = dev_get_drvdata(dev);
276
+	struct packet_router *apr = dev_get_drvdata(dev);
263
277
 	struct apr_device *adev = NULL;
278
+	struct pkt_router_svc *svc;
264
279
 	int ret;
265
280
 
266
281
 	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
267
282
 	if (!adev)
268
283
 		return -ENOMEM;
269
284
 
270
-	spin_lock_init(&adev->lock);
285
+	adev->svc_id = svc_id;
286
+	svc = &adev->svc;
271
287
 
272
-	adev->svc_id = id->svc_id;
273
-	adev->domain_id = id->domain_id;
274
-	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
+
275
296
 	if (np)
276
297
 		snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
277
-	else
278
-		strscpy(adev->name, id->name, APR_NAME_SIZE);
279
298
 
280
-	dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
281
-		     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
+	}
282
307
 
283
308
 	adev->dev.bus = &aprbus;
284
309
 	adev->dev.parent = dev;
..
..
@@ -287,12 +312,20 @@
287
312
 	adev->dev.driver = NULL;
288
313
 
289
314
 	spin_lock(&apr->svcs_lock);
290
-	idr_alloc(&apr->svcs_idr, adev, id->svc_id,
291
-		  id->svc_id + 1, GFP_ATOMIC);
315
+	ret = idr_alloc(&apr->svcs_idr, svc, svc_id, svc_id + 1, GFP_ATOMIC);
292
316
 	spin_unlock(&apr->svcs_lock);
317
+	if (ret < 0) {
318
+		dev_err(dev, "idr_alloc failed: %d\n", ret);
319
+		goto out;
320
+	}
293
321
 
294
-	of_property_read_string_index(np, "qcom,protection-domain",
295
-				      1, &adev->service_path);
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
+	}
296
329
 
297
330
 	dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
298
331
 
..
..
@@ -302,13 +335,14 @@
302
335
 		put_device(&adev->dev);
303
336
 	}
304
337
 
338
+out:
305
339
 	return ret;
306
340
 }
307
341
 
308
342
 static int of_apr_add_pd_lookups(struct device *dev)
309
343
 {
310
344
 	const char *service_name, *service_path;
311
-	struct apr *apr = dev_get_drvdata(dev);
345
+	struct packet_router *apr = dev_get_drvdata(dev);
312
346
 	struct device_node *node;
313
347
 	struct pdr_service *pds;
314
348
 	int ret;
..
..
@@ -340,13 +374,14 @@
340
374
 
341
375
 static void of_register_apr_devices(struct device *dev, const char *svc_path)
342
376
 {
343
-	struct apr *apr = dev_get_drvdata(dev);
377
+	struct packet_router *apr = dev_get_drvdata(dev);
344
378
 	struct device_node *node;
345
379
 	const char *service_path;
346
380
 	int ret;
347
381
 
348
382
 	for_each_child_of_node(dev->of_node, node) {
349
-		struct apr_device_id id = { {0} };
383
+		u32 svc_id;
384
+		u32 domain_id;
350
385
 
351
386
 		/*
352
387
 		 * This function is called with svc_path NULL during
..
..
@@ -376,13 +411,13 @@
376
411
 				continue;
377
412
 		}
378
413
 
379
-		if (of_property_read_u32(node, "reg", &id.svc_id))
414
+		if (of_property_read_u32(node, "reg", &svc_id))
380
415
 			continue;
381
416
 
382
-		id.domain_id = apr->dest_domain_id;
417
+		domain_id = apr->dest_domain_id;
383
418
 
384
-		if (apr_add_device(dev, node, &id))
385
-			dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
419
+		if (apr_add_device(dev, node, svc_id, domain_id))
420
+			dev_err(dev, "Failed to add apr %d svc\n", svc_id);
386
421
 	}
387
422
 }
388
423
 
..
..
@@ -402,7 +437,7 @@
402
437
 
403
438
 static void apr_pd_status(int state, char *svc_path, void *priv)
404
439
 {
405
-	struct apr *apr = (struct apr *)priv;
440
+	struct packet_router *apr = (struct packet_router *)priv;
406
441
 
407
442
 	switch (state) {
408
443
 	case SERVREG_SERVICE_STATE_UP:
..
..
@@ -417,16 +452,20 @@
417
452
 static int apr_probe(struct rpmsg_device *rpdev)
418
453
 {
419
454
 	struct device *dev = &rpdev->dev;
420
-	struct apr *apr;
455
+	struct packet_router *apr;
421
456
 	int ret;
422
457
 
423
458
 	apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
424
459
 	if (!apr)
425
460
 		return -ENOMEM;
426
461
 
427
-	ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", &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;
428
467
 	if (ret) {
429
-		dev_err(dev, "APR Domain ID not specified in DT\n");
468
+		dev_err(dev, "Domain ID not specified in DT\n");
430
469
 		return ret;
431
470
 	}
432
471
 
..
..
@@ -469,7 +508,7 @@
469
508
 
470
509
 static void apr_remove(struct rpmsg_device *rpdev)
471
510
 {
472
-	struct apr *apr = dev_get_drvdata(&rpdev->dev);
511
+	struct packet_router *apr = dev_get_drvdata(&rpdev->dev);
473
512
 
474
513
 	pdr_handle_release(apr->pdr);
475
514
 	device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
..
..
@@ -506,20 +545,20 @@
506
545
 }
507
546
 EXPORT_SYMBOL_GPL(apr_driver_unregister);
508
547
 
509
-static const struct of_device_id apr_of_match[] = {
548
+static const struct of_device_id pkt_router_of_match[] = {
510
549
 	{ .compatible = "qcom,apr"},
511
550
 	{ .compatible = "qcom,apr-v2"},
512
551
 	{}
513
552
 };
514
-MODULE_DEVICE_TABLE(of, apr_of_match);
553
+MODULE_DEVICE_TABLE(of, pkt_router_of_match);
515
554
 
516
-static struct rpmsg_driver apr_driver = {
555
+static struct rpmsg_driver packet_router_driver = {
517
556
 	.probe = apr_probe,
518
557
 	.remove = apr_remove,
519
558
 	.callback = apr_callback,
520
559
 	.drv = {
521
560
 		.name = "qcom,apr",
522
-		.of_match_table = apr_of_match,
561
+		.of_match_table = pkt_router_of_match,
523
562
 	},
524
563
 };
525
564
 
..
..
@@ -529,7 +568,7 @@
529
568
 
530
569
 	ret = bus_register(&aprbus);
531
570
 	if (!ret)
532
-		ret = register_rpmsg_driver(&apr_driver);
571
+		ret = register_rpmsg_driver(&packet_router_driver);
533
572
 	else
534
573
 		bus_unregister(&aprbus);
535
574
 
..
..
@@ -539,7 +578,7 @@
539
578
 static void __exit apr_exit(void)
540
579
 {
541
580
 	bus_unregister(&aprbus);
542
-	unregister_rpmsg_driver(&apr_driver);
581
+	unregister_rpmsg_driver(&packet_router_driver);
543
582
 }
544
583
 
545
584
 subsys_initcall(apr_init);