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 if (radix_tree_insert(&nodes, node_id, node)) {
94 static int server_match(const struct qrtr_server *srv,
95 const struct qrtr_server_filter *f)
97 unsigned int ifilter = f->ifilter;
99 if (f->service != 0 && srv->service != f->service)
101 if (!ifilter && f->instance)
104 return (srv->instance & ifilter) == f->instance;
107 static int service_announce_new(struct sockaddr_qrtr *dest,
108 struct qrtr_server *srv)
110 struct qrtr_ctrl_pkt pkt;
111 struct msghdr msg = { };
114 trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
115 srv->node, srv->port);
118 iv.iov_len = sizeof(pkt);
120 memset(&pkt, 0, sizeof(pkt));
121 pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
122 pkt.server.service = cpu_to_le32(srv->service);
123 pkt.server.instance = cpu_to_le32(srv->instance);
124 pkt.server.node = cpu_to_le32(srv->node);
125 pkt.server.port = cpu_to_le32(srv->port);
127 msg.msg_name = (struct sockaddr *)dest;
128 msg.msg_namelen = sizeof(*dest);
130 return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
133 static int service_announce_del(struct sockaddr_qrtr *dest,
134 struct qrtr_server *srv)
136 struct qrtr_ctrl_pkt pkt;
137 struct msghdr msg = { };
141 trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
142 srv->node, srv->port);
145 iv.iov_len = sizeof(pkt);
147 memset(&pkt, 0, sizeof(pkt));
148 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
149 pkt.server.service = cpu_to_le32(srv->service);
150 pkt.server.instance = cpu_to_le32(srv->instance);
151 pkt.server.node = cpu_to_le32(srv->node);
152 pkt.server.port = cpu_to_le32(srv->port);
154 msg.msg_name = (struct sockaddr *)dest;
155 msg.msg_namelen = sizeof(*dest);
157 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
159 pr_err("failed to announce del service\n");
164 static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
167 struct qrtr_ctrl_pkt pkt;
168 struct msghdr msg = { };
173 iv.iov_len = sizeof(pkt);
175 memset(&pkt, 0, sizeof(pkt));
176 pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
177 cpu_to_le32(QRTR_TYPE_DEL_SERVER);
179 pkt.server.service = cpu_to_le32(srv->service);
180 pkt.server.instance = cpu_to_le32(srv->instance);
181 pkt.server.node = cpu_to_le32(srv->node);
182 pkt.server.port = cpu_to_le32(srv->port);
185 msg.msg_name = (struct sockaddr *)to;
186 msg.msg_namelen = sizeof(*to);
188 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
190 pr_err("failed to send lookup notification\n");
193 static int announce_servers(struct sockaddr_qrtr *sq)
195 struct radix_tree_iter iter;
196 struct qrtr_server *srv;
197 struct qrtr_node *node;
201 node = node_get(qrtr_ns.local_node);
206 /* Announce the list of servers registered in this node */
207 radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
208 srv = radix_tree_deref_slot(slot);
211 if (radix_tree_deref_retry(srv)) {
212 slot = radix_tree_iter_retry(&iter);
215 slot = radix_tree_iter_resume(slot, &iter);
218 ret = service_announce_new(sq, srv);
220 pr_err("failed to announce new service\n");
232 static struct qrtr_server *server_add(unsigned int service,
233 unsigned int instance,
234 unsigned int node_id,
237 struct qrtr_server *srv;
238 struct qrtr_server *old;
239 struct qrtr_node *node;
241 if (!service || !port)
244 srv = kzalloc(sizeof(*srv), GFP_KERNEL);
248 srv->service = service;
249 srv->instance = instance;
253 node = node_get(node_id);
257 /* Delete the old server on the same port */
258 old = radix_tree_lookup(&node->servers, port);
260 radix_tree_delete(&node->servers, port);
264 radix_tree_insert(&node->servers, port, srv);
266 trace_qrtr_ns_server_add(srv->service, srv->instance,
267 srv->node, srv->port);
276 static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
278 struct qrtr_lookup *lookup;
279 struct qrtr_server *srv;
280 struct list_head *li;
282 srv = radix_tree_lookup(&node->servers, port);
286 radix_tree_delete(&node->servers, port);
288 /* Broadcast the removal of local servers */
289 if (srv->node == qrtr_ns.local_node && bcast)
290 service_announce_del(&qrtr_ns.bcast_sq, srv);
292 /* Announce the service's disappearance to observers */
293 list_for_each(li, &qrtr_ns.lookups) {
294 lookup = container_of(li, struct qrtr_lookup, li);
295 if (lookup->service && lookup->service != srv->service)
297 if (lookup->instance && lookup->instance != srv->instance)
300 lookup_notify(&lookup->sq, srv, false);
308 static int say_hello(struct sockaddr_qrtr *dest)
310 struct qrtr_ctrl_pkt pkt;
311 struct msghdr msg = { };
316 iv.iov_len = sizeof(pkt);
318 memset(&pkt, 0, sizeof(pkt));
319 pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
321 msg.msg_name = (struct sockaddr *)dest;
322 msg.msg_namelen = sizeof(*dest);
324 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
326 pr_err("failed to send hello msg\n");
331 /* Announce the list of servers registered on the local node */
332 static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
340 return announce_servers(sq);
343 static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
345 struct qrtr_node *local_node;
346 struct radix_tree_iter iter;
347 struct qrtr_ctrl_pkt pkt;
348 struct qrtr_server *srv;
349 struct sockaddr_qrtr sq;
350 struct msghdr msg = { };
351 struct qrtr_node *node;
357 iv.iov_len = sizeof(pkt);
359 node = node_get(from->sq_node);
364 /* Advertise removal of this client to all servers of remote node */
365 radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
366 srv = radix_tree_deref_slot(slot);
369 if (radix_tree_deref_retry(srv)) {
370 slot = radix_tree_iter_retry(&iter);
373 slot = radix_tree_iter_resume(slot, &iter);
375 server_del(node, srv->port, true);
380 /* Advertise the removal of this client to all local servers */
381 local_node = node_get(qrtr_ns.local_node);
385 memset(&pkt, 0, sizeof(pkt));
386 pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
387 pkt.client.node = cpu_to_le32(from->sq_node);
390 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
391 srv = radix_tree_deref_slot(slot);
394 if (radix_tree_deref_retry(srv)) {
395 slot = radix_tree_iter_retry(&iter);
398 slot = radix_tree_iter_resume(slot, &iter);
401 sq.sq_family = AF_QIPCRTR;
402 sq.sq_node = srv->node;
403 sq.sq_port = srv->port;
405 msg.msg_name = (struct sockaddr *)&sq;
406 msg.msg_namelen = sizeof(sq);
408 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
410 pr_err("failed to send bye cmd\n");
421 static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
422 unsigned int node_id, unsigned int port)
424 struct qrtr_node *local_node;
425 struct radix_tree_iter iter;
426 struct qrtr_lookup *lookup;
427 struct qrtr_ctrl_pkt pkt;
428 struct msghdr msg = { };
429 struct qrtr_server *srv;
430 struct sockaddr_qrtr sq;
431 struct qrtr_node *node;
432 struct list_head *tmp;
433 struct list_head *li;
439 iv.iov_len = sizeof(pkt);
441 /* Don't accept spoofed messages */
442 if (from->sq_node != node_id)
445 /* Local DEL_CLIENT messages comes from the port being closed */
446 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
449 /* Remove any lookups by this client */
450 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
451 lookup = container_of(li, struct qrtr_lookup, li);
452 if (lookup->sq.sq_node != node_id)
454 if (lookup->sq.sq_port != port)
457 list_del(&lookup->li);
461 /* Remove the server belonging to this port but don't broadcast
462 * DEL_SERVER. Neighbours would've already removed the server belonging
463 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
465 node = node_get(node_id);
467 server_del(node, port, false);
469 /* Advertise the removal of this client to all local servers */
470 local_node = node_get(qrtr_ns.local_node);
474 memset(&pkt, 0, sizeof(pkt));
475 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
476 pkt.client.node = cpu_to_le32(node_id);
477 pkt.client.port = cpu_to_le32(port);
480 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
481 srv = radix_tree_deref_slot(slot);
484 if (radix_tree_deref_retry(srv)) {
485 slot = radix_tree_iter_retry(&iter);
488 slot = radix_tree_iter_resume(slot, &iter);
491 sq.sq_family = AF_QIPCRTR;
492 sq.sq_node = srv->node;
493 sq.sq_port = srv->port;
495 msg.msg_name = (struct sockaddr *)&sq;
496 msg.msg_namelen = sizeof(sq);
498 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
500 pr_err("failed to send del client cmd\n");
511 static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
512 unsigned int service, unsigned int instance,
513 unsigned int node_id, unsigned int port)
515 struct qrtr_lookup *lookup;
516 struct qrtr_server *srv;
517 struct list_head *li;
520 /* Ignore specified node and port for local servers */
521 if (from->sq_node == qrtr_ns.local_node) {
522 node_id = from->sq_node;
523 port = from->sq_port;
526 srv = server_add(service, instance, node_id, port);
530 if (srv->node == qrtr_ns.local_node) {
531 ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
533 pr_err("failed to announce new service\n");
538 /* Notify any potential lookups about the new server */
539 list_for_each(li, &qrtr_ns.lookups) {
540 lookup = container_of(li, struct qrtr_lookup, li);
541 if (lookup->service && lookup->service != service)
543 if (lookup->instance && lookup->instance != instance)
546 lookup_notify(&lookup->sq, srv, true);
552 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
553 unsigned int service, unsigned int instance,
554 unsigned int node_id, unsigned int port)
556 struct qrtr_node *node;
558 /* Ignore specified node and port for local servers*/
559 if (from->sq_node == qrtr_ns.local_node) {
560 node_id = from->sq_node;
561 port = from->sq_port;
564 /* Local servers may only unregister themselves */
565 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
568 node = node_get(node_id);
572 server_del(node, port, true);
577 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
578 unsigned int service, unsigned int instance)
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;
588 /* Accept only local observers */
589 if (from->sq_node != qrtr_ns.local_node)
592 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
597 lookup->service = service;
598 lookup->instance = instance;
599 list_add_tail(&lookup->li, &qrtr_ns.lookups);
601 memset(&filter, 0, sizeof(filter));
602 filter.service = service;
603 filter.instance = instance;
606 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
607 node = radix_tree_deref_slot(node_slot);
610 if (radix_tree_deref_retry(node)) {
611 node_slot = radix_tree_iter_retry(&node_iter);
614 node_slot = radix_tree_iter_resume(node_slot, &node_iter);
616 radix_tree_for_each_slot(srv_slot, &node->servers,
618 struct qrtr_server *srv;
620 srv = radix_tree_deref_slot(srv_slot);
623 if (radix_tree_deref_retry(srv)) {
624 srv_slot = radix_tree_iter_retry(&srv_iter);
628 if (!server_match(srv, &filter))
631 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
634 lookup_notify(from, srv, true);
640 /* Empty notification, to indicate end of listing */
641 lookup_notify(from, NULL, true);
646 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
647 unsigned int service, unsigned int instance)
649 struct qrtr_lookup *lookup;
650 struct list_head *tmp;
651 struct list_head *li;
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)
657 if (lookup->sq.sq_port != from->sq_port)
659 if (lookup->service != service)
661 if (lookup->instance && lookup->instance != instance)
664 list_del(&lookup->li);
669 static void qrtr_ns_worker(struct work_struct *work)
671 const struct qrtr_ctrl_pkt *pkt;
672 size_t recv_buf_size = 4096;
673 struct sockaddr_qrtr sq;
674 struct msghdr msg = { };
681 msg.msg_name = (struct sockaddr *)&sq;
682 msg.msg_namelen = sizeof(sq);
684 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
689 iv.iov_base = recv_buf;
690 iv.iov_len = recv_buf_size;
692 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
693 iv.iov_len, MSG_DONTWAIT);
695 if (msglen == -EAGAIN)
699 pr_err("error receiving packet: %zd\n", msglen);
704 cmd = le32_to_cpu(pkt->cmd);
705 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
706 qrtr_ctrl_pkt_strings[cmd])
707 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
708 sq.sq_node, sq.sq_port);
712 case QRTR_TYPE_HELLO:
713 ret = ctrl_cmd_hello(&sq);
716 ret = ctrl_cmd_bye(&sq);
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));
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));
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));
739 case QRTR_TYPE_RESUME_TX:
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));
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));
754 pr_err("failed while handling packet from %d:%d",
755 sq.sq_node, sq.sq_port);
761 static void qrtr_ns_data_ready(struct sock *sk)
763 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
766 int qrtr_ns_init(void)
768 struct sockaddr_qrtr sq;
771 INIT_LIST_HEAD(&qrtr_ns.lookups);
772 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
774 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
775 PF_QIPCRTR, &qrtr_ns.sock);
779 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
781 pr_err("failed to get socket name\n");
785 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
786 if (!qrtr_ns.workqueue) {
791 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
793 sq.sq_port = QRTR_PORT_CTRL;
794 qrtr_ns.local_node = sq.sq_node;
796 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
798 pr_err("failed to bind to socket\n");
802 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
803 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
804 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
806 ret = say_hello(&qrtr_ns.bcast_sq);
813 destroy_workqueue(qrtr_ns.workqueue);
815 sock_release(qrtr_ns.sock);
818 EXPORT_SYMBOL_GPL(qrtr_ns_init);
820 void qrtr_ns_remove(void)
822 cancel_work_sync(&qrtr_ns.work);
823 destroy_workqueue(qrtr_ns.workqueue);
824 sock_release(qrtr_ns.sock);
826 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
828 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
829 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
830 MODULE_LICENSE("Dual BSD/GPL");