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