1 | // SPDX-License-Identifier: GPL-2.0 |
2 | // |
3 | // Copyright 2019 Google LLC. |
4 | |
5 | #include <linux/kernel.h> |
6 | #include <linux/module.h> |
7 | #include <linux/of.h> |
8 | #include <linux/platform_device.h> |
9 | #include <linux/remoteproc.h> |
10 | #include <linux/rpmsg/mtk_rpmsg.h> |
11 | #include <linux/slab.h> |
12 | #include <linux/workqueue.h> |
13 | |
14 | #include "rpmsg_internal.h" |
15 | |
16 | struct mtk_rpmsg_rproc_subdev { |
17 | struct platform_device *pdev; |
18 | struct mtk_rpmsg_info *info; |
19 | struct rpmsg_endpoint *ns_ept; |
20 | struct rproc_subdev subdev; |
21 | |
22 | struct work_struct register_work; |
23 | struct list_head channels; |
24 | struct mutex channels_lock; |
25 | }; |
26 | |
27 | #define to_mtk_subdev(d) container_of(d, struct mtk_rpmsg_rproc_subdev, subdev) |
28 | |
29 | struct mtk_rpmsg_channel_info { |
30 | struct rpmsg_channel_info info; |
31 | bool registered; |
32 | struct list_head list; |
33 | }; |
34 | |
35 | /** |
36 | * struct rpmsg_ns_msg - dynamic name service announcement message |
37 | * @name: name of remote service that is published |
38 | * @addr: address of remote service that is published |
39 | * |
40 | * This message is sent across to publish a new service. When we receive these |
41 | * messages, an appropriate rpmsg channel (i.e device) is created. In turn, the |
42 | * ->probe() handler of the appropriate rpmsg driver will be invoked |
43 | * (if/as-soon-as one is registered). |
44 | */ |
45 | struct rpmsg_ns_msg { |
46 | char name[RPMSG_NAME_SIZE]; |
47 | u32 addr; |
48 | } __packed; |
49 | |
50 | struct mtk_rpmsg_device { |
51 | struct rpmsg_device rpdev; |
52 | struct mtk_rpmsg_rproc_subdev *mtk_subdev; |
53 | }; |
54 | |
55 | struct mtk_rpmsg_endpoint { |
56 | struct rpmsg_endpoint ept; |
57 | struct mtk_rpmsg_rproc_subdev *mtk_subdev; |
58 | }; |
59 | |
60 | #define to_mtk_rpmsg_device(r) container_of(r, struct mtk_rpmsg_device, rpdev) |
61 | #define to_mtk_rpmsg_endpoint(r) container_of(r, struct mtk_rpmsg_endpoint, ept) |
62 | |
63 | static const struct rpmsg_endpoint_ops mtk_rpmsg_endpoint_ops; |
64 | |
65 | static void __mtk_ept_release(struct kref *kref) |
66 | { |
67 | struct rpmsg_endpoint *ept = container_of(kref, struct rpmsg_endpoint, |
68 | refcount); |
69 | kfree(to_mtk_rpmsg_endpoint(ept)); |
70 | } |
71 | |
72 | static void mtk_rpmsg_ipi_handler(void *data, unsigned int len, void *priv) |
73 | { |
74 | struct mtk_rpmsg_endpoint *mept = priv; |
75 | struct rpmsg_endpoint *ept = &mept->ept; |
76 | int ret; |
77 | |
78 | ret = (*ept->cb)(ept->rpdev, data, len, ept->priv, ept->addr); |
79 | if (ret) |
80 | dev_warn(&ept->rpdev->dev, "rpmsg handler return error = %d" , |
81 | ret); |
82 | } |
83 | |
84 | static struct rpmsg_endpoint * |
85 | __mtk_create_ept(struct mtk_rpmsg_rproc_subdev *mtk_subdev, |
86 | struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv, |
87 | u32 id) |
88 | { |
89 | struct mtk_rpmsg_endpoint *mept; |
90 | struct rpmsg_endpoint *ept; |
91 | struct platform_device *pdev = mtk_subdev->pdev; |
92 | int ret; |
93 | |
94 | mept = kzalloc(size: sizeof(*mept), GFP_KERNEL); |
95 | if (!mept) |
96 | return NULL; |
97 | mept->mtk_subdev = mtk_subdev; |
98 | |
99 | ept = &mept->ept; |
100 | kref_init(kref: &ept->refcount); |
101 | |
102 | ept->rpdev = rpdev; |
103 | ept->cb = cb; |
104 | ept->priv = priv; |
105 | ept->ops = &mtk_rpmsg_endpoint_ops; |
106 | ept->addr = id; |
107 | |
108 | ret = mtk_subdev->info->register_ipi(pdev, id, mtk_rpmsg_ipi_handler, |
109 | mept); |
110 | if (ret) { |
111 | dev_err(&pdev->dev, "IPI register failed, id = %d" , id); |
112 | kref_put(kref: &ept->refcount, release: __mtk_ept_release); |
113 | return NULL; |
114 | } |
115 | |
116 | return ept; |
117 | } |
118 | |
119 | static struct rpmsg_endpoint * |
120 | mtk_rpmsg_create_ept(struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv, |
121 | struct rpmsg_channel_info chinfo) |
122 | { |
123 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = |
124 | to_mtk_rpmsg_device(rpdev)->mtk_subdev; |
125 | |
126 | return __mtk_create_ept(mtk_subdev, rpdev, cb, priv, id: chinfo.src); |
127 | } |
128 | |
129 | static void mtk_rpmsg_destroy_ept(struct rpmsg_endpoint *ept) |
130 | { |
131 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = |
132 | to_mtk_rpmsg_endpoint(ept)->mtk_subdev; |
133 | |
134 | mtk_subdev->info->unregister_ipi(mtk_subdev->pdev, ept->addr); |
135 | kref_put(kref: &ept->refcount, release: __mtk_ept_release); |
136 | } |
137 | |
138 | static int mtk_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len) |
139 | { |
140 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = |
141 | to_mtk_rpmsg_endpoint(ept)->mtk_subdev; |
142 | |
143 | return mtk_subdev->info->send_ipi(mtk_subdev->pdev, ept->addr, data, |
144 | len, 0); |
145 | } |
146 | |
147 | static int mtk_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len) |
148 | { |
149 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = |
150 | to_mtk_rpmsg_endpoint(ept)->mtk_subdev; |
151 | |
152 | /* |
153 | * TODO: This currently is same as mtk_rpmsg_send, and wait until SCP |
154 | * received the last command. |
155 | */ |
156 | return mtk_subdev->info->send_ipi(mtk_subdev->pdev, ept->addr, data, |
157 | len, 0); |
158 | } |
159 | |
160 | static const struct rpmsg_endpoint_ops mtk_rpmsg_endpoint_ops = { |
161 | .destroy_ept = mtk_rpmsg_destroy_ept, |
162 | .send = mtk_rpmsg_send, |
163 | .trysend = mtk_rpmsg_trysend, |
164 | }; |
165 | |
166 | static void mtk_rpmsg_release_device(struct device *dev) |
167 | { |
168 | struct rpmsg_device *rpdev = to_rpmsg_device(dev); |
169 | struct mtk_rpmsg_device *mdev = to_mtk_rpmsg_device(rpdev); |
170 | |
171 | kfree(objp: mdev); |
172 | } |
173 | |
174 | static const struct rpmsg_device_ops mtk_rpmsg_device_ops = { |
175 | .create_ept = mtk_rpmsg_create_ept, |
176 | }; |
177 | |
178 | static struct device_node * |
179 | mtk_rpmsg_match_device_subnode(struct device_node *node, const char *channel) |
180 | { |
181 | struct device_node *child; |
182 | const char *name; |
183 | int ret; |
184 | |
185 | for_each_available_child_of_node(node, child) { |
186 | ret = of_property_read_string(np: child, propname: "mediatek,rpmsg-name" , out_string: &name); |
187 | if (ret) |
188 | continue; |
189 | |
190 | if (strcmp(name, channel) == 0) |
191 | return child; |
192 | } |
193 | |
194 | return NULL; |
195 | } |
196 | |
197 | static int mtk_rpmsg_register_device(struct mtk_rpmsg_rproc_subdev *mtk_subdev, |
198 | struct rpmsg_channel_info *info) |
199 | { |
200 | struct rpmsg_device *rpdev; |
201 | struct mtk_rpmsg_device *mdev; |
202 | struct platform_device *pdev = mtk_subdev->pdev; |
203 | |
204 | mdev = kzalloc(size: sizeof(*mdev), GFP_KERNEL); |
205 | if (!mdev) |
206 | return -ENOMEM; |
207 | |
208 | mdev->mtk_subdev = mtk_subdev; |
209 | |
210 | rpdev = &mdev->rpdev; |
211 | rpdev->ops = &mtk_rpmsg_device_ops; |
212 | rpdev->src = info->src; |
213 | rpdev->dst = info->dst; |
214 | strscpy(p: rpdev->id.name, q: info->name, RPMSG_NAME_SIZE); |
215 | |
216 | rpdev->dev.of_node = |
217 | mtk_rpmsg_match_device_subnode(node: pdev->dev.of_node, channel: info->name); |
218 | rpdev->dev.parent = &pdev->dev; |
219 | rpdev->dev.release = mtk_rpmsg_release_device; |
220 | |
221 | return rpmsg_register_device(rpdev); |
222 | } |
223 | |
224 | static void mtk_register_device_work_function(struct work_struct *register_work) |
225 | { |
226 | struct mtk_rpmsg_rproc_subdev *subdev = container_of( |
227 | register_work, struct mtk_rpmsg_rproc_subdev, register_work); |
228 | struct platform_device *pdev = subdev->pdev; |
229 | struct mtk_rpmsg_channel_info *info; |
230 | int ret; |
231 | |
232 | mutex_lock(&subdev->channels_lock); |
233 | list_for_each_entry(info, &subdev->channels, list) { |
234 | if (info->registered) |
235 | continue; |
236 | |
237 | mutex_unlock(lock: &subdev->channels_lock); |
238 | ret = mtk_rpmsg_register_device(mtk_subdev: subdev, info: &info->info); |
239 | mutex_lock(&subdev->channels_lock); |
240 | if (ret) { |
241 | dev_err(&pdev->dev, "Can't create rpmsg_device\n" ); |
242 | continue; |
243 | } |
244 | |
245 | info->registered = true; |
246 | } |
247 | mutex_unlock(lock: &subdev->channels_lock); |
248 | } |
249 | |
250 | static int mtk_rpmsg_create_device(struct mtk_rpmsg_rproc_subdev *mtk_subdev, |
251 | char *name, u32 addr) |
252 | { |
253 | struct mtk_rpmsg_channel_info *info; |
254 | |
255 | info = kzalloc(size: sizeof(*info), GFP_KERNEL); |
256 | if (!info) |
257 | return -ENOMEM; |
258 | |
259 | strscpy(p: info->info.name, q: name, RPMSG_NAME_SIZE); |
260 | info->info.src = addr; |
261 | info->info.dst = RPMSG_ADDR_ANY; |
262 | mutex_lock(&mtk_subdev->channels_lock); |
263 | list_add(new: &info->list, head: &mtk_subdev->channels); |
264 | mutex_unlock(lock: &mtk_subdev->channels_lock); |
265 | |
266 | schedule_work(work: &mtk_subdev->register_work); |
267 | return 0; |
268 | } |
269 | |
270 | static int mtk_rpmsg_ns_cb(struct rpmsg_device *rpdev, void *data, int len, |
271 | void *priv, u32 src) |
272 | { |
273 | struct rpmsg_ns_msg *msg = data; |
274 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = priv; |
275 | struct device *dev = &mtk_subdev->pdev->dev; |
276 | |
277 | int ret; |
278 | |
279 | if (len != sizeof(*msg)) { |
280 | dev_err(dev, "malformed ns msg (%d)\n" , len); |
281 | return -EINVAL; |
282 | } |
283 | |
284 | /* |
285 | * the name service ept does _not_ belong to a real rpmsg channel, |
286 | * and is handled by the rpmsg bus itself. |
287 | * for sanity reasons, make sure a valid rpdev has _not_ sneaked |
288 | * in somehow. |
289 | */ |
290 | if (rpdev) { |
291 | dev_err(dev, "anomaly: ns ept has an rpdev handle\n" ); |
292 | return -EINVAL; |
293 | } |
294 | |
295 | /* don't trust the remote processor for null terminating the name */ |
296 | msg->name[RPMSG_NAME_SIZE - 1] = '\0'; |
297 | |
298 | dev_info(dev, "creating channel %s addr 0x%x\n" , msg->name, msg->addr); |
299 | |
300 | ret = mtk_rpmsg_create_device(mtk_subdev, name: msg->name, addr: msg->addr); |
301 | if (ret) { |
302 | dev_err(dev, "create rpmsg device failed\n" ); |
303 | return ret; |
304 | } |
305 | |
306 | return 0; |
307 | } |
308 | |
309 | static int mtk_rpmsg_prepare(struct rproc_subdev *subdev) |
310 | { |
311 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev); |
312 | |
313 | /* a dedicated endpoint handles the name service msgs */ |
314 | if (mtk_subdev->info->ns_ipi_id >= 0) { |
315 | mtk_subdev->ns_ept = |
316 | __mtk_create_ept(mtk_subdev, NULL, cb: mtk_rpmsg_ns_cb, |
317 | priv: mtk_subdev, |
318 | id: mtk_subdev->info->ns_ipi_id); |
319 | if (!mtk_subdev->ns_ept) { |
320 | dev_err(&mtk_subdev->pdev->dev, |
321 | "failed to create name service endpoint\n" ); |
322 | return -ENOMEM; |
323 | } |
324 | } |
325 | |
326 | return 0; |
327 | } |
328 | |
329 | static void mtk_rpmsg_unprepare(struct rproc_subdev *subdev) |
330 | { |
331 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev); |
332 | |
333 | if (mtk_subdev->ns_ept) { |
334 | mtk_rpmsg_destroy_ept(ept: mtk_subdev->ns_ept); |
335 | mtk_subdev->ns_ept = NULL; |
336 | } |
337 | } |
338 | |
339 | static void mtk_rpmsg_stop(struct rproc_subdev *subdev, bool crashed) |
340 | { |
341 | struct mtk_rpmsg_channel_info *info, *next; |
342 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev); |
343 | struct device *dev = &mtk_subdev->pdev->dev; |
344 | |
345 | /* |
346 | * Destroy the name service endpoint here, to avoid new channel being |
347 | * created after the rpmsg_unregister_device loop below. |
348 | */ |
349 | if (mtk_subdev->ns_ept) { |
350 | mtk_rpmsg_destroy_ept(ept: mtk_subdev->ns_ept); |
351 | mtk_subdev->ns_ept = NULL; |
352 | } |
353 | |
354 | cancel_work_sync(work: &mtk_subdev->register_work); |
355 | |
356 | mutex_lock(&mtk_subdev->channels_lock); |
357 | list_for_each_entry(info, &mtk_subdev->channels, list) { |
358 | if (!info->registered) |
359 | continue; |
360 | if (rpmsg_unregister_device(parent: dev, chinfo: &info->info)) { |
361 | dev_warn( |
362 | dev, |
363 | "rpmsg_unregister_device failed for %s.%d.%d\n" , |
364 | info->info.name, info->info.src, |
365 | info->info.dst); |
366 | } |
367 | } |
368 | |
369 | list_for_each_entry_safe(info, next, |
370 | &mtk_subdev->channels, list) { |
371 | list_del(entry: &info->list); |
372 | kfree(objp: info); |
373 | } |
374 | mutex_unlock(lock: &mtk_subdev->channels_lock); |
375 | } |
376 | |
377 | struct rproc_subdev * |
378 | mtk_rpmsg_create_rproc_subdev(struct platform_device *pdev, |
379 | struct mtk_rpmsg_info *info) |
380 | { |
381 | struct mtk_rpmsg_rproc_subdev *mtk_subdev; |
382 | |
383 | mtk_subdev = kzalloc(size: sizeof(*mtk_subdev), GFP_KERNEL); |
384 | if (!mtk_subdev) |
385 | return NULL; |
386 | |
387 | mtk_subdev->pdev = pdev; |
388 | mtk_subdev->subdev.prepare = mtk_rpmsg_prepare; |
389 | mtk_subdev->subdev.stop = mtk_rpmsg_stop; |
390 | mtk_subdev->subdev.unprepare = mtk_rpmsg_unprepare; |
391 | mtk_subdev->info = info; |
392 | INIT_LIST_HEAD(list: &mtk_subdev->channels); |
393 | INIT_WORK(&mtk_subdev->register_work, |
394 | mtk_register_device_work_function); |
395 | mutex_init(&mtk_subdev->channels_lock); |
396 | |
397 | return &mtk_subdev->subdev; |
398 | } |
399 | EXPORT_SYMBOL_GPL(mtk_rpmsg_create_rproc_subdev); |
400 | |
401 | void mtk_rpmsg_destroy_rproc_subdev(struct rproc_subdev *subdev) |
402 | { |
403 | struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev); |
404 | |
405 | kfree(objp: mtk_subdev); |
406 | } |
407 | EXPORT_SYMBOL_GPL(mtk_rpmsg_destroy_rproc_subdev); |
408 | |
409 | MODULE_LICENSE("GPL v2" ); |
410 | MODULE_DESCRIPTION("MediaTek scp rpmsg driver" ); |
411 | |