1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
3 * Copyright (c) 2015, Sony Mobile Communications Inc.
4 * Copyright (c) 2013, The Linux Foundation. All rights reserved.
5 * Copyright (c) 2020, Linaro Ltd.
8 #include <linux/module.h>
9 #include <linux/qrtr.h>
10 #include <linux/workqueue.h>
15 #define CREATE_TRACE_POINTS
16 #include <trace/events/qrtr.h>
18 static RADIX_TREE(nodes
, GFP_KERNEL
);
22 struct sockaddr_qrtr bcast_sq
;
23 struct list_head lookups
;
24 struct workqueue_struct
*workqueue
;
25 struct work_struct work
;
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",
42 struct qrtr_server_filter
{
44 unsigned int instance
;
50 unsigned int instance
;
52 struct sockaddr_qrtr sq
;
58 unsigned int instance
;
68 struct radix_tree_root servers
;
71 static struct qrtr_node
*node_get(unsigned int node_id
)
73 struct qrtr_node
*node
;
75 node
= radix_tree_lookup(&nodes
, node_id
);
79 /* If node didn't exist, allocate and insert it to the tree */
80 node
= kzalloc(sizeof(*node
), GFP_KERNEL
);
86 radix_tree_insert(&nodes
, node_id
, node
);
91 static int server_match(const struct qrtr_server
*srv
,
92 const struct qrtr_server_filter
*f
)
94 unsigned int ifilter
= f
->ifilter
;
96 if (f
->service
!= 0 && srv
->service
!= f
->service
)
98 if (!ifilter
&& f
->instance
)
101 return (srv
->instance
& ifilter
) == f
->instance
;
104 static int service_announce_new(struct sockaddr_qrtr
*dest
,
105 struct qrtr_server
*srv
)
107 struct qrtr_ctrl_pkt pkt
;
108 struct msghdr msg
= { };
111 trace_qrtr_ns_service_announce_new(srv
->service
, srv
->instance
,
112 srv
->node
, srv
->port
);
115 iv
.iov_len
= sizeof(pkt
);
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
);
124 msg
.msg_name
= (struct sockaddr
*)dest
;
125 msg
.msg_namelen
= sizeof(*dest
);
127 return kernel_sendmsg(qrtr_ns
.sock
, &msg
, &iv
, 1, sizeof(pkt
));
130 static int service_announce_del(struct sockaddr_qrtr
*dest
,
131 struct qrtr_server
*srv
)
133 struct qrtr_ctrl_pkt pkt
;
134 struct msghdr msg
= { };
138 trace_qrtr_ns_service_announce_del(srv
->service
, srv
->instance
,
139 srv
->node
, srv
->port
);
142 iv
.iov_len
= sizeof(pkt
);
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
);
151 msg
.msg_name
= (struct sockaddr
*)dest
;
152 msg
.msg_namelen
= sizeof(*dest
);
154 ret
= kernel_sendmsg(qrtr_ns
.sock
, &msg
, &iv
, 1, sizeof(pkt
));
156 pr_err("failed to announce del service\n");
161 static void lookup_notify(struct sockaddr_qrtr
*to
, struct qrtr_server
*srv
,
164 struct qrtr_ctrl_pkt pkt
;
165 struct msghdr msg
= { };
170 iv
.iov_len
= sizeof(pkt
);
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
);
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
);
182 msg
.msg_name
= (struct sockaddr
*)to
;
183 msg
.msg_namelen
= sizeof(*to
);
185 ret
= kernel_sendmsg(qrtr_ns
.sock
, &msg
, &iv
, 1, sizeof(pkt
));
187 pr_err("failed to send lookup notification\n");
190 static int announce_servers(struct sockaddr_qrtr
*sq
)
192 struct radix_tree_iter iter
;
193 struct qrtr_server
*srv
;
194 struct qrtr_node
*node
;
198 node
= node_get(qrtr_ns
.local_node
);
202 /* Announce the list of servers registered in this node */
203 radix_tree_for_each_slot(slot
, &node
->servers
, &iter
, 0) {
204 srv
= radix_tree_deref_slot(slot
);
206 ret
= service_announce_new(sq
, srv
);
208 pr_err("failed to announce new service\n");
216 static struct qrtr_server
*server_add(unsigned int service
,
217 unsigned int instance
,
218 unsigned int node_id
,
221 struct qrtr_server
*srv
;
222 struct qrtr_server
*old
;
223 struct qrtr_node
*node
;
225 if (!service
|| !port
)
228 srv
= kzalloc(sizeof(*srv
), GFP_KERNEL
);
232 srv
->service
= service
;
233 srv
->instance
= instance
;
237 node
= node_get(node_id
);
241 /* Delete the old server on the same port */
242 old
= radix_tree_lookup(&node
->servers
, port
);
244 radix_tree_delete(&node
->servers
, port
);
248 radix_tree_insert(&node
->servers
, port
, srv
);
250 trace_qrtr_ns_server_add(srv
->service
, srv
->instance
,
251 srv
->node
, srv
->port
);
260 static int server_del(struct qrtr_node
*node
, unsigned int port
)
262 struct qrtr_lookup
*lookup
;
263 struct qrtr_server
*srv
;
264 struct list_head
*li
;
266 srv
= radix_tree_lookup(&node
->servers
, port
);
270 radix_tree_delete(&node
->servers
, port
);
272 /* Broadcast the removal of local servers */
273 if (srv
->node
== qrtr_ns
.local_node
)
274 service_announce_del(&qrtr_ns
.bcast_sq
, srv
);
276 /* Announce the service's disappearance to observers */
277 list_for_each(li
, &qrtr_ns
.lookups
) {
278 lookup
= container_of(li
, struct qrtr_lookup
, li
);
279 if (lookup
->service
&& lookup
->service
!= srv
->service
)
281 if (lookup
->instance
&& lookup
->instance
!= srv
->instance
)
284 lookup_notify(&lookup
->sq
, srv
, false);
292 static int say_hello(struct sockaddr_qrtr
*dest
)
294 struct qrtr_ctrl_pkt pkt
;
295 struct msghdr msg
= { };
300 iv
.iov_len
= sizeof(pkt
);
302 memset(&pkt
, 0, sizeof(pkt
));
303 pkt
.cmd
= cpu_to_le32(QRTR_TYPE_HELLO
);
305 msg
.msg_name
= (struct sockaddr
*)dest
;
306 msg
.msg_namelen
= sizeof(*dest
);
308 ret
= kernel_sendmsg(qrtr_ns
.sock
, &msg
, &iv
, 1, sizeof(pkt
));
310 pr_err("failed to send hello msg\n");
315 /* Announce the list of servers registered on the local node */
316 static int ctrl_cmd_hello(struct sockaddr_qrtr
*sq
)
324 return announce_servers(sq
);
327 static int ctrl_cmd_bye(struct sockaddr_qrtr
*from
)
329 struct qrtr_node
*local_node
;
330 struct radix_tree_iter iter
;
331 struct qrtr_ctrl_pkt pkt
;
332 struct qrtr_server
*srv
;
333 struct sockaddr_qrtr sq
;
334 struct msghdr msg
= { };
335 struct qrtr_node
*node
;
341 iv
.iov_len
= sizeof(pkt
);
343 node
= node_get(from
->sq_node
);
347 /* Advertise removal of this client to all servers of remote node */
348 radix_tree_for_each_slot(slot
, &node
->servers
, &iter
, 0) {
349 srv
= radix_tree_deref_slot(slot
);
350 server_del(node
, srv
->port
);
353 /* Advertise the removal of this client to all local servers */
354 local_node
= node_get(qrtr_ns
.local_node
);
358 memset(&pkt
, 0, sizeof(pkt
));
359 pkt
.cmd
= cpu_to_le32(QRTR_TYPE_BYE
);
360 pkt
.client
.node
= cpu_to_le32(from
->sq_node
);
362 radix_tree_for_each_slot(slot
, &local_node
->servers
, &iter
, 0) {
363 srv
= radix_tree_deref_slot(slot
);
365 sq
.sq_family
= AF_QIPCRTR
;
366 sq
.sq_node
= srv
->node
;
367 sq
.sq_port
= srv
->port
;
369 msg
.msg_name
= (struct sockaddr
*)&sq
;
370 msg
.msg_namelen
= sizeof(sq
);
372 ret
= kernel_sendmsg(qrtr_ns
.sock
, &msg
, &iv
, 1, sizeof(pkt
));
374 pr_err("failed to send bye cmd\n");
382 static int ctrl_cmd_del_client(struct sockaddr_qrtr
*from
,
383 unsigned int node_id
, unsigned int port
)
385 struct qrtr_node
*local_node
;
386 struct radix_tree_iter iter
;
387 struct qrtr_lookup
*lookup
;
388 struct qrtr_ctrl_pkt pkt
;
389 struct msghdr msg
= { };
390 struct qrtr_server
*srv
;
391 struct sockaddr_qrtr sq
;
392 struct qrtr_node
*node
;
393 struct list_head
*tmp
;
394 struct list_head
*li
;
400 iv
.iov_len
= sizeof(pkt
);
402 /* Don't accept spoofed messages */
403 if (from
->sq_node
!= node_id
)
406 /* Local DEL_CLIENT messages comes from the port being closed */
407 if (from
->sq_node
== qrtr_ns
.local_node
&& from
->sq_port
!= port
)
410 /* Remove any lookups by this client */
411 list_for_each_safe(li
, tmp
, &qrtr_ns
.lookups
) {
412 lookup
= container_of(li
, struct qrtr_lookup
, li
);
413 if (lookup
->sq
.sq_node
!= node_id
)
415 if (lookup
->sq
.sq_port
!= port
)
418 list_del(&lookup
->li
);
422 /* Remove the server belonging to this port */
423 node
= node_get(node_id
);
425 server_del(node
, port
);
427 /* Advertise the removal of this client to all local servers */
428 local_node
= node_get(qrtr_ns
.local_node
);
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
);
437 radix_tree_for_each_slot(slot
, &local_node
->servers
, &iter
, 0) {
438 srv
= radix_tree_deref_slot(slot
);
440 sq
.sq_family
= AF_QIPCRTR
;
441 sq
.sq_node
= srv
->node
;
442 sq
.sq_port
= srv
->port
;
444 msg
.msg_name
= (struct sockaddr
*)&sq
;
445 msg
.msg_namelen
= sizeof(sq
);
447 ret
= kernel_sendmsg(qrtr_ns
.sock
, &msg
, &iv
, 1, sizeof(pkt
));
449 pr_err("failed to send del client cmd\n");
457 static int ctrl_cmd_new_server(struct sockaddr_qrtr
*from
,
458 unsigned int service
, unsigned int instance
,
459 unsigned int node_id
, unsigned int port
)
461 struct qrtr_lookup
*lookup
;
462 struct qrtr_server
*srv
;
463 struct list_head
*li
;
466 /* Ignore specified node and port for local servers */
467 if (from
->sq_node
== qrtr_ns
.local_node
) {
468 node_id
= from
->sq_node
;
469 port
= from
->sq_port
;
472 /* Don't accept spoofed messages */
473 if (from
->sq_node
!= node_id
)
476 srv
= server_add(service
, instance
, node_id
, port
);
480 if (srv
->node
== qrtr_ns
.local_node
) {
481 ret
= service_announce_new(&qrtr_ns
.bcast_sq
, srv
);
483 pr_err("failed to announce new service\n");
488 /* Notify any potential lookups about the new server */
489 list_for_each(li
, &qrtr_ns
.lookups
) {
490 lookup
= container_of(li
, struct qrtr_lookup
, li
);
491 if (lookup
->service
&& lookup
->service
!= service
)
493 if (lookup
->instance
&& lookup
->instance
!= instance
)
496 lookup_notify(&lookup
->sq
, srv
, true);
502 static int ctrl_cmd_del_server(struct sockaddr_qrtr
*from
,
503 unsigned int service
, unsigned int instance
,
504 unsigned int node_id
, unsigned int port
)
506 struct qrtr_node
*node
;
508 /* Ignore specified node and port for local servers*/
509 if (from
->sq_node
== qrtr_ns
.local_node
) {
510 node_id
= from
->sq_node
;
511 port
= from
->sq_port
;
514 /* Don't accept spoofed messages */
515 if (from
->sq_node
!= node_id
)
518 /* Local servers may only unregister themselves */
519 if (from
->sq_node
== qrtr_ns
.local_node
&& from
->sq_port
!= port
)
522 node
= node_get(node_id
);
526 return server_del(node
, port
);
529 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr
*from
,
530 unsigned int service
, unsigned int instance
)
532 struct radix_tree_iter node_iter
;
533 struct qrtr_server_filter filter
;
534 struct radix_tree_iter srv_iter
;
535 struct qrtr_lookup
*lookup
;
536 struct qrtr_node
*node
;
537 void __rcu
**node_slot
;
538 void __rcu
**srv_slot
;
540 /* Accept only local observers */
541 if (from
->sq_node
!= qrtr_ns
.local_node
)
544 lookup
= kzalloc(sizeof(*lookup
), GFP_KERNEL
);
549 lookup
->service
= service
;
550 lookup
->instance
= instance
;
551 list_add_tail(&lookup
->li
, &qrtr_ns
.lookups
);
553 memset(&filter
, 0, sizeof(filter
));
554 filter
.service
= service
;
555 filter
.instance
= instance
;
557 radix_tree_for_each_slot(node_slot
, &nodes
, &node_iter
, 0) {
558 node
= radix_tree_deref_slot(node_slot
);
560 radix_tree_for_each_slot(srv_slot
, &node
->servers
,
562 struct qrtr_server
*srv
;
564 srv
= radix_tree_deref_slot(srv_slot
);
565 if (!server_match(srv
, &filter
))
568 lookup_notify(from
, srv
, true);
572 /* Empty notification, to indicate end of listing */
573 lookup_notify(from
, NULL
, true);
578 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr
*from
,
579 unsigned int service
, unsigned int instance
)
581 struct qrtr_lookup
*lookup
;
582 struct list_head
*tmp
;
583 struct list_head
*li
;
585 list_for_each_safe(li
, tmp
, &qrtr_ns
.lookups
) {
586 lookup
= container_of(li
, struct qrtr_lookup
, li
);
587 if (lookup
->sq
.sq_node
!= from
->sq_node
)
589 if (lookup
->sq
.sq_port
!= from
->sq_port
)
591 if (lookup
->service
!= service
)
593 if (lookup
->instance
&& lookup
->instance
!= instance
)
596 list_del(&lookup
->li
);
601 static void qrtr_ns_worker(struct work_struct
*work
)
603 const struct qrtr_ctrl_pkt
*pkt
;
604 size_t recv_buf_size
= 4096;
605 struct sockaddr_qrtr sq
;
606 struct msghdr msg
= { };
613 msg
.msg_name
= (struct sockaddr
*)&sq
;
614 msg
.msg_namelen
= sizeof(sq
);
616 recv_buf
= kzalloc(recv_buf_size
, GFP_KERNEL
);
621 iv
.iov_base
= recv_buf
;
622 iv
.iov_len
= recv_buf_size
;
624 msglen
= kernel_recvmsg(qrtr_ns
.sock
, &msg
, &iv
, 1,
625 iv
.iov_len
, MSG_DONTWAIT
);
627 if (msglen
== -EAGAIN
)
631 pr_err("error receiving packet: %zd\n", msglen
);
636 cmd
= le32_to_cpu(pkt
->cmd
);
637 if (cmd
< ARRAY_SIZE(qrtr_ctrl_pkt_strings
) &&
638 qrtr_ctrl_pkt_strings
[cmd
])
639 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings
[cmd
],
640 sq
.sq_node
, sq
.sq_port
);
644 case QRTR_TYPE_HELLO
:
645 ret
= ctrl_cmd_hello(&sq
);
648 ret
= ctrl_cmd_bye(&sq
);
650 case QRTR_TYPE_DEL_CLIENT
:
651 ret
= ctrl_cmd_del_client(&sq
,
652 le32_to_cpu(pkt
->client
.node
),
653 le32_to_cpu(pkt
->client
.port
));
655 case QRTR_TYPE_NEW_SERVER
:
656 ret
= ctrl_cmd_new_server(&sq
,
657 le32_to_cpu(pkt
->server
.service
),
658 le32_to_cpu(pkt
->server
.instance
),
659 le32_to_cpu(pkt
->server
.node
),
660 le32_to_cpu(pkt
->server
.port
));
662 case QRTR_TYPE_DEL_SERVER
:
663 ret
= ctrl_cmd_del_server(&sq
,
664 le32_to_cpu(pkt
->server
.service
),
665 le32_to_cpu(pkt
->server
.instance
),
666 le32_to_cpu(pkt
->server
.node
),
667 le32_to_cpu(pkt
->server
.port
));
671 case QRTR_TYPE_RESUME_TX
:
673 case QRTR_TYPE_NEW_LOOKUP
:
674 ret
= ctrl_cmd_new_lookup(&sq
,
675 le32_to_cpu(pkt
->server
.service
),
676 le32_to_cpu(pkt
->server
.instance
));
678 case QRTR_TYPE_DEL_LOOKUP
:
679 ctrl_cmd_del_lookup(&sq
,
680 le32_to_cpu(pkt
->server
.service
),
681 le32_to_cpu(pkt
->server
.instance
));
686 pr_err("failed while handling packet from %d:%d",
687 sq
.sq_node
, sq
.sq_port
);
693 static void qrtr_ns_data_ready(struct sock
*sk
)
695 queue_work(qrtr_ns
.workqueue
, &qrtr_ns
.work
);
698 void qrtr_ns_init(void)
700 struct sockaddr_qrtr sq
;
703 INIT_LIST_HEAD(&qrtr_ns
.lookups
);
704 INIT_WORK(&qrtr_ns
.work
, qrtr_ns_worker
);
706 ret
= sock_create_kern(&init_net
, AF_QIPCRTR
, SOCK_DGRAM
,
707 PF_QIPCRTR
, &qrtr_ns
.sock
);
711 ret
= kernel_getsockname(qrtr_ns
.sock
, (struct sockaddr
*)&sq
);
713 pr_err("failed to get socket name\n");
717 qrtr_ns
.workqueue
= alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND
, 1);
718 if (!qrtr_ns
.workqueue
)
721 qrtr_ns
.sock
->sk
->sk_data_ready
= qrtr_ns_data_ready
;
723 sq
.sq_port
= QRTR_PORT_CTRL
;
724 qrtr_ns
.local_node
= sq
.sq_node
;
726 ret
= kernel_bind(qrtr_ns
.sock
, (struct sockaddr
*)&sq
, sizeof(sq
));
728 pr_err("failed to bind to socket\n");
732 qrtr_ns
.bcast_sq
.sq_family
= AF_QIPCRTR
;
733 qrtr_ns
.bcast_sq
.sq_node
= QRTR_NODE_BCAST
;
734 qrtr_ns
.bcast_sq
.sq_port
= QRTR_PORT_CTRL
;
736 ret
= say_hello(&qrtr_ns
.bcast_sq
);
743 destroy_workqueue(qrtr_ns
.workqueue
);
745 sock_release(qrtr_ns
.sock
);
747 EXPORT_SYMBOL_GPL(qrtr_ns_init
);
749 void qrtr_ns_remove(void)
751 cancel_work_sync(&qrtr_ns
.work
);
752 destroy_workqueue(qrtr_ns
.workqueue
);
753 sock_release(qrtr_ns
.sock
);
755 EXPORT_SYMBOL_GPL(qrtr_ns_remove
);
757 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
758 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
759 MODULE_LICENSE("Dual BSD/GPL");