Commit | Line | Data |
---|---|---|
0c2204a4 MS |
1 | // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause |
2 | /* | |
3 | * Copyright (c) 2015, Sony Mobile Communications Inc. | |
4 | * Copyright (c) 2013, The Linux Foundation. All rights reserved. | |
5 | * Copyright (c) 2020, Linaro Ltd. | |
6 | */ | |
7 | ||
8 | #include <linux/module.h> | |
9 | #include <linux/qrtr.h> | |
10 | #include <linux/workqueue.h> | |
11 | #include <net/sock.h> | |
12 | ||
13 | #include "qrtr.h" | |
14 | ||
40e0b090 | 15 | #include <trace/events/sock.h> |
dfddb540 MS |
16 | #define CREATE_TRACE_POINTS |
17 | #include <trace/events/qrtr.h> | |
18 | ||
0c2204a4 MS |
19 | static RADIX_TREE(nodes, GFP_KERNEL); |
20 | ||
21 | static struct { | |
22 | struct socket *sock; | |
23 | struct sockaddr_qrtr bcast_sq; | |
24 | struct list_head lookups; | |
25 | struct workqueue_struct *workqueue; | |
26 | struct work_struct work; | |
27 | int local_node; | |
28 | } qrtr_ns; | |
29 | ||
30 | static const char * const qrtr_ctrl_pkt_strings[] = { | |
31 | [QRTR_TYPE_HELLO] = "hello", | |
32 | [QRTR_TYPE_BYE] = "bye", | |
33 | [QRTR_TYPE_NEW_SERVER] = "new-server", | |
34 | [QRTR_TYPE_DEL_SERVER] = "del-server", | |
35 | [QRTR_TYPE_DEL_CLIENT] = "del-client", | |
36 | [QRTR_TYPE_RESUME_TX] = "resume-tx", | |
37 | [QRTR_TYPE_EXIT] = "exit", | |
38 | [QRTR_TYPE_PING] = "ping", | |
39 | [QRTR_TYPE_NEW_LOOKUP] = "new-lookup", | |
40 | [QRTR_TYPE_DEL_LOOKUP] = "del-lookup", | |
41 | }; | |
42 | ||
43 | struct qrtr_server_filter { | |
44 | unsigned int service; | |
45 | unsigned int instance; | |
46 | unsigned int ifilter; | |
47 | }; | |
48 | ||
49 | struct qrtr_lookup { | |
50 | unsigned int service; | |
51 | unsigned int instance; | |
52 | ||
53 | struct sockaddr_qrtr sq; | |
54 | struct list_head li; | |
55 | }; | |
56 | ||
57 | struct qrtr_server { | |
58 | unsigned int service; | |
59 | unsigned int instance; | |
60 | ||
61 | unsigned int node; | |
62 | unsigned int port; | |
63 | ||
64 | struct list_head qli; | |
65 | }; | |
66 | ||
67 | struct qrtr_node { | |
68 | unsigned int id; | |
69 | struct radix_tree_root servers; | |
70 | }; | |
71 | ||
72 | static struct qrtr_node *node_get(unsigned int node_id) | |
73 | { | |
74 | struct qrtr_node *node; | |
75 | ||
76 | node = radix_tree_lookup(&nodes, node_id); | |
77 | if (node) | |
78 | return node; | |
79 | ||
80 | /* If node didn't exist, allocate and insert it to the tree */ | |
81 | node = kzalloc(sizeof(*node), GFP_KERNEL); | |
82 | if (!node) | |
9baeea50 | 83 | return NULL; |
0c2204a4 MS |
84 | |
85 | node->id = node_id; | |
86 | ||
29de68c2 NP |
87 | if (radix_tree_insert(&nodes, node_id, node)) { |
88 | kfree(node); | |
89 | return NULL; | |
90 | } | |
0c2204a4 MS |
91 | |
92 | return node; | |
93 | } | |
94 | ||
95 | static int server_match(const struct qrtr_server *srv, | |
96 | const struct qrtr_server_filter *f) | |
97 | { | |
98 | unsigned int ifilter = f->ifilter; | |
99 | ||
100 | if (f->service != 0 && srv->service != f->service) | |
101 | return 0; | |
102 | if (!ifilter && f->instance) | |
103 | ifilter = ~0; | |
104 | ||
105 | return (srv->instance & ifilter) == f->instance; | |
106 | } | |
107 | ||
108 | static int service_announce_new(struct sockaddr_qrtr *dest, | |
109 | struct qrtr_server *srv) | |
110 | { | |
111 | struct qrtr_ctrl_pkt pkt; | |
112 | struct msghdr msg = { }; | |
113 | struct kvec iv; | |
114 | ||
dfddb540 MS |
115 | trace_qrtr_ns_service_announce_new(srv->service, srv->instance, |
116 | srv->node, srv->port); | |
0c2204a4 MS |
117 | |
118 | iv.iov_base = &pkt; | |
119 | iv.iov_len = sizeof(pkt); | |
120 | ||
121 | memset(&pkt, 0, sizeof(pkt)); | |
122 | pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); | |
123 | pkt.server.service = cpu_to_le32(srv->service); | |
124 | pkt.server.instance = cpu_to_le32(srv->instance); | |
125 | pkt.server.node = cpu_to_le32(srv->node); | |
126 | pkt.server.port = cpu_to_le32(srv->port); | |
127 | ||
128 | msg.msg_name = (struct sockaddr *)dest; | |
129 | msg.msg_namelen = sizeof(*dest); | |
130 | ||
131 | return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | |
132 | } | |
133 | ||
134 | static int service_announce_del(struct sockaddr_qrtr *dest, | |
135 | struct qrtr_server *srv) | |
136 | { | |
137 | struct qrtr_ctrl_pkt pkt; | |
138 | struct msghdr msg = { }; | |
139 | struct kvec iv; | |
140 | int ret; | |
141 | ||
dfddb540 MS |
142 | trace_qrtr_ns_service_announce_del(srv->service, srv->instance, |
143 | srv->node, srv->port); | |
0c2204a4 MS |
144 | |
145 | iv.iov_base = &pkt; | |
146 | iv.iov_len = sizeof(pkt); | |
147 | ||
148 | memset(&pkt, 0, sizeof(pkt)); | |
149 | pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER); | |
150 | pkt.server.service = cpu_to_le32(srv->service); | |
151 | pkt.server.instance = cpu_to_le32(srv->instance); | |
152 | pkt.server.node = cpu_to_le32(srv->node); | |
153 | pkt.server.port = cpu_to_le32(srv->port); | |
154 | ||
155 | msg.msg_name = (struct sockaddr *)dest; | |
156 | msg.msg_namelen = sizeof(*dest); | |
157 | ||
158 | ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | |
159 | if (ret < 0) | |
13ef6ae8 | 160 | pr_err("failed to announce del service\n"); |
0c2204a4 MS |
161 | |
162 | return ret; | |
163 | } | |
164 | ||
165 | static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv, | |
166 | bool new) | |
167 | { | |
168 | struct qrtr_ctrl_pkt pkt; | |
169 | struct msghdr msg = { }; | |
170 | struct kvec iv; | |
171 | int ret; | |
172 | ||
173 | iv.iov_base = &pkt; | |
174 | iv.iov_len = sizeof(pkt); | |
175 | ||
176 | memset(&pkt, 0, sizeof(pkt)); | |
177 | pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) : | |
178 | cpu_to_le32(QRTR_TYPE_DEL_SERVER); | |
179 | if (srv) { | |
180 | pkt.server.service = cpu_to_le32(srv->service); | |
181 | pkt.server.instance = cpu_to_le32(srv->instance); | |
182 | pkt.server.node = cpu_to_le32(srv->node); | |
183 | pkt.server.port = cpu_to_le32(srv->port); | |
184 | } | |
185 | ||
186 | msg.msg_name = (struct sockaddr *)to; | |
187 | msg.msg_namelen = sizeof(*to); | |
188 | ||
189 | ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | |
190 | if (ret < 0) | |
191 | pr_err("failed to send lookup notification\n"); | |
192 | } | |
193 | ||
194 | static int announce_servers(struct sockaddr_qrtr *sq) | |
195 | { | |
196 | struct radix_tree_iter iter; | |
197 | struct qrtr_server *srv; | |
198 | struct qrtr_node *node; | |
199 | void __rcu **slot; | |
082bb94f | 200 | int ret; |
0c2204a4 MS |
201 | |
202 | node = node_get(qrtr_ns.local_node); | |
203 | if (!node) | |
204 | return 0; | |
205 | ||
a7809ff9 | 206 | rcu_read_lock(); |
0c2204a4 MS |
207 | /* Announce the list of servers registered in this node */ |
208 | radix_tree_for_each_slot(slot, &node->servers, &iter, 0) { | |
209 | srv = radix_tree_deref_slot(slot); | |
082bb94f MS |
210 | if (!srv) |
211 | continue; | |
212 | if (radix_tree_deref_retry(srv)) { | |
213 | slot = radix_tree_iter_retry(&iter); | |
214 | continue; | |
215 | } | |
216 | slot = radix_tree_iter_resume(slot, &iter); | |
217 | rcu_read_unlock(); | |
0c2204a4 MS |
218 | |
219 | ret = service_announce_new(sq, srv); | |
220 | if (ret < 0) { | |
221 | pr_err("failed to announce new service\n"); | |
082bb94f | 222 | return ret; |
0c2204a4 | 223 | } |
082bb94f MS |
224 | |
225 | rcu_read_lock(); | |
0c2204a4 MS |
226 | } |
227 | ||
a7809ff9 MS |
228 | rcu_read_unlock(); |
229 | ||
082bb94f | 230 | return 0; |
0c2204a4 MS |
231 | } |
232 | ||
233 | static struct qrtr_server *server_add(unsigned int service, | |
234 | unsigned int instance, | |
235 | unsigned int node_id, | |
236 | unsigned int port) | |
237 | { | |
238 | struct qrtr_server *srv; | |
239 | struct qrtr_server *old; | |
240 | struct qrtr_node *node; | |
241 | ||
242 | if (!service || !port) | |
243 | return NULL; | |
244 | ||
245 | srv = kzalloc(sizeof(*srv), GFP_KERNEL); | |
246 | if (!srv) | |
9baeea50 | 247 | return NULL; |
0c2204a4 MS |
248 | |
249 | srv->service = service; | |
250 | srv->instance = instance; | |
251 | srv->node = node_id; | |
252 | srv->port = port; | |
253 | ||
254 | node = node_get(node_id); | |
255 | if (!node) | |
256 | goto err; | |
257 | ||
258 | /* Delete the old server on the same port */ | |
259 | old = radix_tree_lookup(&node->servers, port); | |
260 | if (old) { | |
261 | radix_tree_delete(&node->servers, port); | |
262 | kfree(old); | |
263 | } | |
264 | ||
265 | radix_tree_insert(&node->servers, port, srv); | |
266 | ||
dfddb540 MS |
267 | trace_qrtr_ns_server_add(srv->service, srv->instance, |
268 | srv->node, srv->port); | |
0c2204a4 MS |
269 | |
270 | return srv; | |
271 | ||
272 | err: | |
273 | kfree(srv); | |
274 | return NULL; | |
275 | } | |
276 | ||
277 | static int server_del(struct qrtr_node *node, unsigned int port) | |
278 | { | |
279 | struct qrtr_lookup *lookup; | |
280 | struct qrtr_server *srv; | |
281 | struct list_head *li; | |
282 | ||
283 | srv = radix_tree_lookup(&node->servers, port); | |
284 | if (!srv) | |
285 | return -ENOENT; | |
286 | ||
287 | radix_tree_delete(&node->servers, port); | |
288 | ||
289 | /* Broadcast the removal of local servers */ | |
290 | if (srv->node == qrtr_ns.local_node) | |
291 | service_announce_del(&qrtr_ns.bcast_sq, srv); | |
292 | ||
293 | /* Announce the service's disappearance to observers */ | |
294 | list_for_each(li, &qrtr_ns.lookups) { | |
295 | lookup = container_of(li, struct qrtr_lookup, li); | |
296 | if (lookup->service && lookup->service != srv->service) | |
297 | continue; | |
298 | if (lookup->instance && lookup->instance != srv->instance) | |
299 | continue; | |
300 | ||
301 | lookup_notify(&lookup->sq, srv, false); | |
302 | } | |
303 | ||
304 | kfree(srv); | |
305 | ||
306 | return 0; | |
307 | } | |
308 | ||
a1dc1d6a BA |
309 | static int say_hello(struct sockaddr_qrtr *dest) |
310 | { | |
311 | struct qrtr_ctrl_pkt pkt; | |
312 | struct msghdr msg = { }; | |
313 | struct kvec iv; | |
314 | int ret; | |
315 | ||
316 | iv.iov_base = &pkt; | |
317 | iv.iov_len = sizeof(pkt); | |
318 | ||
319 | memset(&pkt, 0, sizeof(pkt)); | |
320 | pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO); | |
321 | ||
322 | msg.msg_name = (struct sockaddr *)dest; | |
323 | msg.msg_namelen = sizeof(*dest); | |
324 | ||
325 | ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | |
326 | if (ret < 0) | |
327 | pr_err("failed to send hello msg\n"); | |
328 | ||
329 | return ret; | |
330 | } | |
331 | ||
0c2204a4 MS |
332 | /* Announce the list of servers registered on the local node */ |
333 | static int ctrl_cmd_hello(struct sockaddr_qrtr *sq) | |
334 | { | |
a1dc1d6a BA |
335 | int ret; |
336 | ||
337 | ret = say_hello(sq); | |
338 | if (ret < 0) | |
339 | return ret; | |
340 | ||
0c2204a4 MS |
341 | return announce_servers(sq); |
342 | } | |
343 | ||
344 | static int ctrl_cmd_bye(struct sockaddr_qrtr *from) | |
345 | { | |
346 | struct qrtr_node *local_node; | |
347 | struct radix_tree_iter iter; | |
348 | struct qrtr_ctrl_pkt pkt; | |
349 | struct qrtr_server *srv; | |
350 | struct sockaddr_qrtr sq; | |
351 | struct msghdr msg = { }; | |
352 | struct qrtr_node *node; | |
353 | void __rcu **slot; | |
354 | struct kvec iv; | |
082bb94f | 355 | int ret; |
0c2204a4 MS |
356 | |
357 | iv.iov_base = &pkt; | |
358 | iv.iov_len = sizeof(pkt); | |
359 | ||
360 | node = node_get(from->sq_node); | |
361 | if (!node) | |
362 | return 0; | |
363 | ||
a7809ff9 | 364 | rcu_read_lock(); |
0c2204a4 MS |
365 | /* Advertise removal of this client to all servers of remote node */ |
366 | radix_tree_for_each_slot(slot, &node->servers, &iter, 0) { | |
367 | srv = radix_tree_deref_slot(slot); | |
082bb94f MS |
368 | if (!srv) |
369 | continue; | |
370 | if (radix_tree_deref_retry(srv)) { | |
371 | slot = radix_tree_iter_retry(&iter); | |
372 | continue; | |
373 | } | |
374 | slot = radix_tree_iter_resume(slot, &iter); | |
375 | rcu_read_unlock(); | |
0c2204a4 | 376 | server_del(node, srv->port); |
082bb94f | 377 | rcu_read_lock(); |
0c2204a4 | 378 | } |
a7809ff9 | 379 | rcu_read_unlock(); |
0c2204a4 MS |
380 | |
381 | /* Advertise the removal of this client to all local servers */ | |
382 | local_node = node_get(qrtr_ns.local_node); | |
383 | if (!local_node) | |
384 | return 0; | |
385 | ||
386 | memset(&pkt, 0, sizeof(pkt)); | |
387 | pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE); | |
388 | pkt.client.node = cpu_to_le32(from->sq_node); | |
389 | ||
a7809ff9 | 390 | rcu_read_lock(); |
0c2204a4 MS |
391 | radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { |
392 | srv = radix_tree_deref_slot(slot); | |
082bb94f MS |
393 | if (!srv) |
394 | continue; | |
395 | if (radix_tree_deref_retry(srv)) { | |
396 | slot = radix_tree_iter_retry(&iter); | |
397 | continue; | |
398 | } | |
399 | slot = radix_tree_iter_resume(slot, &iter); | |
400 | rcu_read_unlock(); | |
0c2204a4 MS |
401 | |
402 | sq.sq_family = AF_QIPCRTR; | |
403 | sq.sq_node = srv->node; | |
404 | sq.sq_port = srv->port; | |
405 | ||
406 | msg.msg_name = (struct sockaddr *)&sq; | |
407 | msg.msg_namelen = sizeof(sq); | |
408 | ||
409 | ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | |
410 | if (ret < 0) { | |
411 | pr_err("failed to send bye cmd\n"); | |
082bb94f | 412 | return ret; |
0c2204a4 | 413 | } |
082bb94f | 414 | rcu_read_lock(); |
0c2204a4 MS |
415 | } |
416 | ||
a7809ff9 MS |
417 | rcu_read_unlock(); |
418 | ||
082bb94f | 419 | return 0; |
0c2204a4 MS |
420 | } |
421 | ||
422 | static int ctrl_cmd_del_client(struct sockaddr_qrtr *from, | |
423 | unsigned int node_id, unsigned int port) | |
424 | { | |
425 | struct qrtr_node *local_node; | |
426 | struct radix_tree_iter iter; | |
427 | struct qrtr_lookup *lookup; | |
428 | struct qrtr_ctrl_pkt pkt; | |
429 | struct msghdr msg = { }; | |
430 | struct qrtr_server *srv; | |
431 | struct sockaddr_qrtr sq; | |
432 | struct qrtr_node *node; | |
433 | struct list_head *tmp; | |
434 | struct list_head *li; | |
435 | void __rcu **slot; | |
436 | struct kvec iv; | |
082bb94f | 437 | int ret; |
0c2204a4 MS |
438 | |
439 | iv.iov_base = &pkt; | |
440 | iv.iov_len = sizeof(pkt); | |
441 | ||
442 | /* Don't accept spoofed messages */ | |
443 | if (from->sq_node != node_id) | |
444 | return -EINVAL; | |
445 | ||
446 | /* Local DEL_CLIENT messages comes from the port being closed */ | |
447 | if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) | |
448 | return -EINVAL; | |
449 | ||
450 | /* Remove any lookups by this client */ | |
451 | list_for_each_safe(li, tmp, &qrtr_ns.lookups) { | |
452 | lookup = container_of(li, struct qrtr_lookup, li); | |
453 | if (lookup->sq.sq_node != node_id) | |
454 | continue; | |
455 | if (lookup->sq.sq_port != port) | |
456 | continue; | |
457 | ||
458 | list_del(&lookup->li); | |
459 | kfree(lookup); | |
460 | } | |
461 | ||
462 | /* Remove the server belonging to this port */ | |
463 | node = node_get(node_id); | |
464 | if (node) | |
465 | server_del(node, port); | |
466 | ||
467 | /* Advertise the removal of this client to all local servers */ | |
468 | local_node = node_get(qrtr_ns.local_node); | |
469 | if (!local_node) | |
470 | return 0; | |
471 | ||
472 | memset(&pkt, 0, sizeof(pkt)); | |
473 | pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT); | |
474 | pkt.client.node = cpu_to_le32(node_id); | |
475 | pkt.client.port = cpu_to_le32(port); | |
476 | ||
a7809ff9 | 477 | rcu_read_lock(); |
0c2204a4 MS |
478 | radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { |
479 | srv = radix_tree_deref_slot(slot); | |
082bb94f MS |
480 | if (!srv) |
481 | continue; | |
482 | if (radix_tree_deref_retry(srv)) { | |
483 | slot = radix_tree_iter_retry(&iter); | |
484 | continue; | |
485 | } | |
486 | slot = radix_tree_iter_resume(slot, &iter); | |
487 | rcu_read_unlock(); | |
0c2204a4 MS |
488 | |
489 | sq.sq_family = AF_QIPCRTR; | |
490 | sq.sq_node = srv->node; | |
491 | sq.sq_port = srv->port; | |
492 | ||
493 | msg.msg_name = (struct sockaddr *)&sq; | |
494 | msg.msg_namelen = sizeof(sq); | |
495 | ||
496 | ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | |
497 | if (ret < 0) { | |
498 | pr_err("failed to send del client cmd\n"); | |
082bb94f | 499 | return ret; |
0c2204a4 | 500 | } |
082bb94f | 501 | rcu_read_lock(); |
0c2204a4 MS |
502 | } |
503 | ||
a7809ff9 MS |
504 | rcu_read_unlock(); |
505 | ||
082bb94f | 506 | return 0; |
0c2204a4 MS |
507 | } |
508 | ||
509 | static int ctrl_cmd_new_server(struct sockaddr_qrtr *from, | |
510 | unsigned int service, unsigned int instance, | |
511 | unsigned int node_id, unsigned int port) | |
512 | { | |
513 | struct qrtr_lookup *lookup; | |
514 | struct qrtr_server *srv; | |
515 | struct list_head *li; | |
516 | int ret = 0; | |
517 | ||
518 | /* Ignore specified node and port for local servers */ | |
519 | if (from->sq_node == qrtr_ns.local_node) { | |
520 | node_id = from->sq_node; | |
521 | port = from->sq_port; | |
522 | } | |
523 | ||
0c2204a4 MS |
524 | srv = server_add(service, instance, node_id, port); |
525 | if (!srv) | |
526 | return -EINVAL; | |
527 | ||
528 | if (srv->node == qrtr_ns.local_node) { | |
529 | ret = service_announce_new(&qrtr_ns.bcast_sq, srv); | |
530 | if (ret < 0) { | |
531 | pr_err("failed to announce new service\n"); | |
532 | return ret; | |
533 | } | |
534 | } | |
535 | ||
536 | /* Notify any potential lookups about the new server */ | |
537 | list_for_each(li, &qrtr_ns.lookups) { | |
538 | lookup = container_of(li, struct qrtr_lookup, li); | |
539 | if (lookup->service && lookup->service != service) | |
540 | continue; | |
541 | if (lookup->instance && lookup->instance != instance) | |
542 | continue; | |
543 | ||
544 | lookup_notify(&lookup->sq, srv, true); | |
545 | } | |
546 | ||
547 | return ret; | |
548 | } | |
549 | ||
550 | static int ctrl_cmd_del_server(struct sockaddr_qrtr *from, | |
551 | unsigned int service, unsigned int instance, | |
552 | unsigned int node_id, unsigned int port) | |
553 | { | |
554 | struct qrtr_node *node; | |
555 | ||
556 | /* Ignore specified node and port for local servers*/ | |
557 | if (from->sq_node == qrtr_ns.local_node) { | |
558 | node_id = from->sq_node; | |
559 | port = from->sq_port; | |
560 | } | |
561 | ||
0c2204a4 MS |
562 | /* Local servers may only unregister themselves */ |
563 | if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) | |
564 | return -EINVAL; | |
565 | ||
566 | node = node_get(node_id); | |
567 | if (!node) | |
568 | return -ENOENT; | |
569 | ||
570 | return server_del(node, port); | |
571 | } | |
572 | ||
573 | static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from, | |
574 | unsigned int service, unsigned int instance) | |
575 | { | |
576 | struct radix_tree_iter node_iter; | |
577 | struct qrtr_server_filter filter; | |
578 | struct radix_tree_iter srv_iter; | |
579 | struct qrtr_lookup *lookup; | |
580 | struct qrtr_node *node; | |
581 | void __rcu **node_slot; | |
582 | void __rcu **srv_slot; | |
583 | ||
584 | /* Accept only local observers */ | |
585 | if (from->sq_node != qrtr_ns.local_node) | |
586 | return -EINVAL; | |
587 | ||
588 | lookup = kzalloc(sizeof(*lookup), GFP_KERNEL); | |
589 | if (!lookup) | |
590 | return -ENOMEM; | |
591 | ||
592 | lookup->sq = *from; | |
593 | lookup->service = service; | |
594 | lookup->instance = instance; | |
595 | list_add_tail(&lookup->li, &qrtr_ns.lookups); | |
596 | ||
597 | memset(&filter, 0, sizeof(filter)); | |
598 | filter.service = service; | |
599 | filter.instance = instance; | |
600 | ||
a7809ff9 | 601 | rcu_read_lock(); |
0c2204a4 MS |
602 | radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) { |
603 | node = radix_tree_deref_slot(node_slot); | |
082bb94f MS |
604 | if (!node) |
605 | continue; | |
606 | if (radix_tree_deref_retry(node)) { | |
607 | node_slot = radix_tree_iter_retry(&node_iter); | |
608 | continue; | |
609 | } | |
610 | node_slot = radix_tree_iter_resume(node_slot, &node_iter); | |
0c2204a4 MS |
611 | |
612 | radix_tree_for_each_slot(srv_slot, &node->servers, | |
613 | &srv_iter, 0) { | |
614 | struct qrtr_server *srv; | |
615 | ||
616 | srv = radix_tree_deref_slot(srv_slot); | |
082bb94f MS |
617 | if (!srv) |
618 | continue; | |
619 | if (radix_tree_deref_retry(srv)) { | |
620 | srv_slot = radix_tree_iter_retry(&srv_iter); | |
621 | continue; | |
622 | } | |
623 | ||
0c2204a4 MS |
624 | if (!server_match(srv, &filter)) |
625 | continue; | |
626 | ||
082bb94f MS |
627 | srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter); |
628 | ||
629 | rcu_read_unlock(); | |
0c2204a4 | 630 | lookup_notify(from, srv, true); |
082bb94f | 631 | rcu_read_lock(); |
0c2204a4 MS |
632 | } |
633 | } | |
a7809ff9 | 634 | rcu_read_unlock(); |
0c2204a4 MS |
635 | |
636 | /* Empty notification, to indicate end of listing */ | |
637 | lookup_notify(from, NULL, true); | |
638 | ||
639 | return 0; | |
640 | } | |
641 | ||
642 | static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from, | |
643 | unsigned int service, unsigned int instance) | |
644 | { | |
645 | struct qrtr_lookup *lookup; | |
646 | struct list_head *tmp; | |
647 | struct list_head *li; | |
648 | ||
649 | list_for_each_safe(li, tmp, &qrtr_ns.lookups) { | |
650 | lookup = container_of(li, struct qrtr_lookup, li); | |
651 | if (lookup->sq.sq_node != from->sq_node) | |
652 | continue; | |
653 | if (lookup->sq.sq_port != from->sq_port) | |
654 | continue; | |
655 | if (lookup->service != service) | |
656 | continue; | |
657 | if (lookup->instance && lookup->instance != instance) | |
658 | continue; | |
659 | ||
660 | list_del(&lookup->li); | |
661 | kfree(lookup); | |
662 | } | |
663 | } | |
664 | ||
0c2204a4 MS |
665 | static void qrtr_ns_worker(struct work_struct *work) |
666 | { | |
667 | const struct qrtr_ctrl_pkt *pkt; | |
668 | size_t recv_buf_size = 4096; | |
669 | struct sockaddr_qrtr sq; | |
670 | struct msghdr msg = { }; | |
671 | unsigned int cmd; | |
672 | ssize_t msglen; | |
673 | void *recv_buf; | |
674 | struct kvec iv; | |
675 | int ret; | |
676 | ||
677 | msg.msg_name = (struct sockaddr *)&sq; | |
678 | msg.msg_namelen = sizeof(sq); | |
679 | ||
680 | recv_buf = kzalloc(recv_buf_size, GFP_KERNEL); | |
681 | if (!recv_buf) | |
682 | return; | |
683 | ||
684 | for (;;) { | |
685 | iv.iov_base = recv_buf; | |
686 | iv.iov_len = recv_buf_size; | |
687 | ||
688 | msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1, | |
689 | iv.iov_len, MSG_DONTWAIT); | |
690 | ||
691 | if (msglen == -EAGAIN) | |
692 | break; | |
693 | ||
694 | if (msglen < 0) { | |
695 | pr_err("error receiving packet: %zd\n", msglen); | |
696 | break; | |
697 | } | |
698 | ||
699 | pkt = recv_buf; | |
700 | cmd = le32_to_cpu(pkt->cmd); | |
701 | if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) && | |
702 | qrtr_ctrl_pkt_strings[cmd]) | |
dfddb540 MS |
703 | trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd], |
704 | sq.sq_node, sq.sq_port); | |
0c2204a4 MS |
705 | |
706 | ret = 0; | |
707 | switch (cmd) { | |
708 | case QRTR_TYPE_HELLO: | |
709 | ret = ctrl_cmd_hello(&sq); | |
710 | break; | |
711 | case QRTR_TYPE_BYE: | |
712 | ret = ctrl_cmd_bye(&sq); | |
713 | break; | |
714 | case QRTR_TYPE_DEL_CLIENT: | |
715 | ret = ctrl_cmd_del_client(&sq, | |
716 | le32_to_cpu(pkt->client.node), | |
717 | le32_to_cpu(pkt->client.port)); | |
718 | break; | |
719 | case QRTR_TYPE_NEW_SERVER: | |
720 | ret = ctrl_cmd_new_server(&sq, | |
721 | le32_to_cpu(pkt->server.service), | |
722 | le32_to_cpu(pkt->server.instance), | |
723 | le32_to_cpu(pkt->server.node), | |
724 | le32_to_cpu(pkt->server.port)); | |
725 | break; | |
726 | case QRTR_TYPE_DEL_SERVER: | |
727 | ret = ctrl_cmd_del_server(&sq, | |
728 | le32_to_cpu(pkt->server.service), | |
729 | le32_to_cpu(pkt->server.instance), | |
730 | le32_to_cpu(pkt->server.node), | |
731 | le32_to_cpu(pkt->server.port)); | |
732 | break; | |
733 | case QRTR_TYPE_EXIT: | |
734 | case QRTR_TYPE_PING: | |
735 | case QRTR_TYPE_RESUME_TX: | |
736 | break; | |
737 | case QRTR_TYPE_NEW_LOOKUP: | |
738 | ret = ctrl_cmd_new_lookup(&sq, | |
739 | le32_to_cpu(pkt->server.service), | |
740 | le32_to_cpu(pkt->server.instance)); | |
741 | break; | |
742 | case QRTR_TYPE_DEL_LOOKUP: | |
743 | ctrl_cmd_del_lookup(&sq, | |
744 | le32_to_cpu(pkt->server.service), | |
745 | le32_to_cpu(pkt->server.instance)); | |
746 | break; | |
747 | } | |
748 | ||
749 | if (ret < 0) | |
750 | pr_err("failed while handling packet from %d:%d", | |
751 | sq.sq_node, sq.sq_port); | |
752 | } | |
753 | ||
754 | kfree(recv_buf); | |
755 | } | |
756 | ||
757 | static void qrtr_ns_data_ready(struct sock *sk) | |
758 | { | |
40e0b090 PY |
759 | trace_sk_data_ready(sk); |
760 | ||
0c2204a4 MS |
761 | queue_work(qrtr_ns.workqueue, &qrtr_ns.work); |
762 | } | |
763 | ||
4beb17e5 | 764 | int qrtr_ns_init(void) |
0c2204a4 MS |
765 | { |
766 | struct sockaddr_qrtr sq; | |
767 | int ret; | |
768 | ||
769 | INIT_LIST_HEAD(&qrtr_ns.lookups); | |
770 | INIT_WORK(&qrtr_ns.work, qrtr_ns_worker); | |
771 | ||
772 | ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM, | |
773 | PF_QIPCRTR, &qrtr_ns.sock); | |
774 | if (ret < 0) | |
4beb17e5 | 775 | return ret; |
0c2204a4 MS |
776 | |
777 | ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq); | |
778 | if (ret < 0) { | |
779 | pr_err("failed to get socket name\n"); | |
780 | goto err_sock; | |
781 | } | |
782 | ||
c6e08d62 | 783 | qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1); |
a49e72b3 WY |
784 | if (!qrtr_ns.workqueue) { |
785 | ret = -ENOMEM; | |
c6e08d62 | 786 | goto err_sock; |
a49e72b3 | 787 | } |
c6e08d62 | 788 | |
0c2204a4 MS |
789 | qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready; |
790 | ||
791 | sq.sq_port = QRTR_PORT_CTRL; | |
792 | qrtr_ns.local_node = sq.sq_node; | |
793 | ||
794 | ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq)); | |
795 | if (ret < 0) { | |
796 | pr_err("failed to bind to socket\n"); | |
c6e08d62 | 797 | goto err_wq; |
0c2204a4 MS |
798 | } |
799 | ||
800 | qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR; | |
801 | qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST; | |
802 | qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL; | |
803 | ||
a1dc1d6a | 804 | ret = say_hello(&qrtr_ns.bcast_sq); |
0c2204a4 MS |
805 | if (ret < 0) |
806 | goto err_wq; | |
807 | ||
4beb17e5 | 808 | return 0; |
0c2204a4 MS |
809 | |
810 | err_wq: | |
811 | destroy_workqueue(qrtr_ns.workqueue); | |
812 | err_sock: | |
813 | sock_release(qrtr_ns.sock); | |
4beb17e5 | 814 | return ret; |
0c2204a4 MS |
815 | } |
816 | EXPORT_SYMBOL_GPL(qrtr_ns_init); | |
817 | ||
818 | void qrtr_ns_remove(void) | |
819 | { | |
820 | cancel_work_sync(&qrtr_ns.work); | |
821 | destroy_workqueue(qrtr_ns.workqueue); | |
822 | sock_release(qrtr_ns.sock); | |
823 | } | |
824 | EXPORT_SYMBOL_GPL(qrtr_ns_remove); | |
825 | ||
826 | MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>"); | |
827 | MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice"); | |
828 | MODULE_LICENSE("Dual BSD/GPL"); |