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