add stmac read mac form eeprom

hc

2023-12-09 b22da3d8526a935aa31e086e63f60ff3246cb61c

 kernel/drivers/soc/qcom/apr.c
..
..
@@ -8,8 +8,10 @@
8
8
 #include <linux/spinlock.h>
9
9
 #include <linux/idr.h>
10
10
 #include <linux/slab.h>
11
+#include <linux/workqueue.h>
11
12
 #include <linux/of_device.h>
12
13
 #include <linux/soc/qcom/apr.h>
14
+#include <linux/soc/qcom/pdr.h>
13
15
 #include <linux/rpmsg.h>
14
16
 #include <linux/of.h>
15
17
 
..
..
@@ -17,8 +19,19 @@
17
19
 	struct rpmsg_endpoint *ch;
18
20
 	struct device *dev;
19
21
 	spinlock_t svcs_lock;
22
+	spinlock_t rx_lock;
20
23
 	struct idr svcs_idr;
21
24
 	int dest_domain_id;
25
+	struct pdr_handle *pdr;
26
+	struct workqueue_struct *rxwq;
27
+	struct work_struct rx_work;
28
+	struct list_head rx_list;
29
+};
30
+
31
+struct apr_rx_buf {
32
+	struct list_head node;
33
+	int len;
34
+	uint8_t buf[];
22
35
 };
23
36
 
24
37
 /**
..
..
@@ -62,11 +75,7 @@
62
75
 				  int len, void *priv, u32 addr)
63
76
 {
64
77
 	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;
78
+	struct apr_rx_buf *abuf;
70
79
 	unsigned long flags;
71
80
 
72
81
 	if (len <= APR_HDR_SIZE) {
..
..
@@ -74,6 +83,34 @@
74
83
 			buf, len);
75
84
 		return -EINVAL;
76
85
 	}
86
+
87
+	abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC);
88
+	if (!abuf)
89
+		return -ENOMEM;
90
+
91
+	abuf->len = len;
92
+	memcpy(abuf->buf, buf, len);
93
+
94
+	spin_lock_irqsave(&apr->rx_lock, flags);
95
+	list_add_tail(&abuf->node, &apr->rx_list);
96
+	spin_unlock_irqrestore(&apr->rx_lock, flags);
97
+
98
+	queue_work(apr->rxwq, &apr->rx_work);
99
+
100
+	return 0;
101
+}
102
+
103
+
104
+static int apr_do_rx_callback(struct apr *apr, struct apr_rx_buf *abuf)
105
+{
106
+	uint16_t hdr_size, msg_type, ver, svc_id;
107
+	struct apr_device *svc = NULL;
108
+	struct apr_driver *adrv = NULL;
109
+	struct apr_resp_pkt resp;
110
+	struct apr_hdr *hdr;
111
+	unsigned long flags;
112
+	void *buf = abuf->buf;
113
+	int len = abuf->len;
77
114
 
78
115
 	hdr = buf;
79
116
 	ver = APR_HDR_FIELD_VER(hdr->hdr_field);
..
..
@@ -87,7 +124,7 @@
87
124
 	}
88
125
 
89
126
 	if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {
90
-		dev_err(apr->dev, "APR: Wrong paket size\n");
127
+		dev_err(apr->dev, "APR: Wrong packet size\n");
91
128
 		return -EINVAL;
92
129
 	}
93
130
 
..
..
@@ -130,6 +167,23 @@
130
167
 	adrv->callback(svc, &resp);
131
168
 
132
169
 	return 0;
170
+}
171
+
172
+static void apr_rxwq(struct work_struct *work)
173
+{
174
+	struct apr *apr = container_of(work, struct apr, rx_work);
175
+	struct apr_rx_buf *abuf, *b;
176
+	unsigned long flags;
177
+
178
+	if (!list_empty(&apr->rx_list)) {
179
+		list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {
180
+			apr_do_rx_callback(apr, abuf);
181
+			spin_lock_irqsave(&apr->rx_lock, flags);
182
+			list_del(&abuf->node);
183
+			spin_unlock_irqrestore(&apr->rx_lock, flags);
184
+			kfree(abuf);
185
+		}
186
+	}
133
187
 }
134
188
 
135
189
 static int apr_device_match(struct device *dev, struct device_driver *drv)
..
..
@@ -219,7 +273,7 @@
219
273
 	adev->domain_id = id->domain_id;
220
274
 	adev->version = id->svc_version;
221
275
 	if (np)
222
-		strscpy(adev->name, np->name, APR_NAME_SIZE);
276
+		snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
223
277
 	else
224
278
 		strscpy(adev->name, id->name, APR_NAME_SIZE);
225
279
 
..
..
@@ -237,6 +291,9 @@
237
291
 		  id->svc_id + 1, GFP_ATOMIC);
238
292
 	spin_unlock(&apr->svcs_lock);
239
293
 
294
+	of_property_read_string_index(np, "qcom,protection-domain",
295
+				      1, &adev->service_path);
296
+
240
297
 	dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
241
298
 
242
299
 	ret = device_register(&adev->dev);
..
..
@@ -248,13 +305,76 @@
248
305
 	return ret;
249
306
 }
250
307
 
251
-static void of_register_apr_devices(struct device *dev)
308
+static int of_apr_add_pd_lookups(struct device *dev)
309
+{
310
+	const char *service_name, *service_path;
311
+	struct apr *apr = dev_get_drvdata(dev);
312
+	struct device_node *node;
313
+	struct pdr_service *pds;
314
+	int ret;
315
+
316
+	for_each_child_of_node(dev->of_node, node) {
317
+		ret = of_property_read_string_index(node, "qcom,protection-domain",
318
+						    0, &service_name);
319
+		if (ret < 0)
320
+			continue;
321
+
322
+		ret = of_property_read_string_index(node, "qcom,protection-domain",
323
+						    1, &service_path);
324
+		if (ret < 0) {
325
+			dev_err(dev, "pdr service path missing: %d\n", ret);
326
+			of_node_put(node);
327
+			return ret;
328
+		}
329
+
330
+		pds = pdr_add_lookup(apr->pdr, service_name, service_path);
331
+		if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
332
+			dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));
333
+			of_node_put(node);
334
+			return PTR_ERR(pds);
335
+		}
336
+	}
337
+
338
+	return 0;
339
+}
340
+
341
+static void of_register_apr_devices(struct device *dev, const char *svc_path)
252
342
 {
253
343
 	struct apr *apr = dev_get_drvdata(dev);
254
344
 	struct device_node *node;
345
+	const char *service_path;
346
+	int ret;
255
347
 
256
348
 	for_each_child_of_node(dev->of_node, node) {
257
349
 		struct apr_device_id id = { {0} };
350
+
351
+		/*
352
+		 * This function is called with svc_path NULL during
353
+		 * apr_probe(), in which case we register any apr devices
354
+		 * without a qcom,protection-domain specified.
355
+		 *
356
+		 * Then as the protection domains becomes available
357
+		 * (if applicable) this function is again called, but with
358
+		 * svc_path representing the service becoming available. In
359
+		 * this case we register any apr devices with a matching
360
+		 * qcom,protection-domain.
361
+		 */
362
+
363
+		ret = of_property_read_string_index(node, "qcom,protection-domain",
364
+						    1, &service_path);
365
+		if (svc_path) {
366
+			/* skip APR services that are PD independent */
367
+			if (ret)
368
+				continue;
369
+
370
+			/* skip APR services whose PD paths don't match */
371
+			if (strcmp(service_path, svc_path))
372
+				continue;
373
+		} else {
374
+			/* skip APR services whose PD lookups are registered */
375
+			if (ret == 0)
376
+				continue;
377
+		}
258
378
 
259
379
 		if (of_property_read_u32(node, "reg", &id.svc_id))
260
380
 			continue;
..
..
@@ -263,6 +383,34 @@
263
383
 
264
384
 		if (apr_add_device(dev, node, &id))
265
385
 			dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
386
+	}
387
+}
388
+
389
+static int apr_remove_device(struct device *dev, void *svc_path)
390
+{
391
+	struct apr_device *adev = to_apr_device(dev);
392
+
393
+	if (svc_path && adev->service_path) {
394
+		if (!strcmp(adev->service_path, (char *)svc_path))
395
+			device_unregister(&adev->dev);
396
+	} else {
397
+		device_unregister(&adev->dev);
398
+	}
399
+
400
+	return 0;
401
+}
402
+
403
+static void apr_pd_status(int state, char *svc_path, void *priv)
404
+{
405
+	struct apr *apr = (struct apr *)priv;
406
+
407
+	switch (state) {
408
+	case SERVREG_SERVICE_STATE_UP:
409
+		of_register_apr_devices(apr->dev, svc_path);
410
+		break;
411
+	case SERVREG_SERVICE_STATE_DOWN:
412
+		device_for_each_child(apr->dev, svc_path, apr_remove_device);
413
+		break;
266
414
 	}
267
415
 }
268
416
 
..
..
@@ -276,7 +424,7 @@
276
424
 	if (!apr)
277
425
 		return -ENOMEM;
278
426
 
279
-	ret = of_property_read_u32(dev->of_node, "reg", &apr->dest_domain_id);
427
+	ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", &apr->dest_domain_id);
280
428
 	if (ret) {
281
429
 		dev_err(dev, "APR Domain ID not specified in DT\n");
282
430
 		return ret;
..
..
@@ -285,25 +433,48 @@
285
433
 	dev_set_drvdata(dev, apr);
286
434
 	apr->ch = rpdev->ept;
287
435
 	apr->dev = dev;
436
+	apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
437
+	if (!apr->rxwq) {
438
+		dev_err(apr->dev, "Failed to start Rx WQ\n");
439
+		return -ENOMEM;
440
+	}
441
+	INIT_WORK(&apr->rx_work, apr_rxwq);
442
+
443
+	apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
444
+	if (IS_ERR(apr->pdr)) {
445
+		dev_err(dev, "Failed to init PDR handle\n");
446
+		ret = PTR_ERR(apr->pdr);
447
+		goto destroy_wq;
448
+	}
449
+
450
+	INIT_LIST_HEAD(&apr->rx_list);
451
+	spin_lock_init(&apr->rx_lock);
288
452
 	spin_lock_init(&apr->svcs_lock);
289
453
 	idr_init(&apr->svcs_idr);
290
-	of_register_apr_devices(dev);
454
+
455
+	ret = of_apr_add_pd_lookups(dev);
456
+	if (ret)
457
+		goto handle_release;
458
+
459
+	of_register_apr_devices(dev, NULL);
291
460
 
292
461
 	return 0;
293
-}
294
462
 
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;
463
+handle_release:
464
+	pdr_handle_release(apr->pdr);
465
+destroy_wq:
466
+	destroy_workqueue(apr->rxwq);
467
+	return ret;
302
468
 }
303
469
 
304
470
 static void apr_remove(struct rpmsg_device *rpdev)
305
471
 {
472
+	struct apr *apr = dev_get_drvdata(&rpdev->dev);
473
+
474
+	pdr_handle_release(apr->pdr);
306
475
 	device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
476
+	flush_workqueue(apr->rxwq);
477
+	destroy_workqueue(apr->rxwq);
307
478
 }
308
479
 
309
480
 /*