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