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
19static DEFINE_XARRAY(nodes);
20
21static 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
30static 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
43struct qrtr_server_filter {
44	unsigned int service;
45	unsigned int instance;
46	unsigned int ifilter;
47};
48
49struct qrtr_lookup {
50	unsigned int service;
51	unsigned int instance;
52
53	struct sockaddr_qrtr sq;
54	struct list_head li;
55};
56
57struct 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
67struct qrtr_node {
68	unsigned int id;
69	struct xarray servers;
70};
71
72static struct qrtr_node *node_get(unsigned int node_id)
73{
74	struct qrtr_node *node;
75
76	node = xa_load(&nodes, 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(sizeof(*node), GFP_KERNEL);
82	if (!node)
83		return NULL;
84
85	node->id = node_id;
86	xa_init(&node->servers);
87
88	if (xa_store(&nodes, node_id, node, GFP_KERNEL)) {
89		kfree(node);
90		return NULL;
91	}
92
93	return node;
94}
95
96static 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
109static 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(srv->service, srv->instance,
117					   srv->node, 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(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
133}
134
135static 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(srv->service, srv->instance,
144					   srv->node, 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(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
160	if (ret < 0)
161		pr_err("failed to announce del service\n");
162
163	return ret;
164}
165
166static 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(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
191	if (ret < 0)
192		pr_err("failed to send lookup notification\n");
193}
194
195static 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(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(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
217static 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(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, port, srv, GFP_KERNEL);
244	if (old) {
245		if (xa_is_err(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(old);
251		}
252	}
253
254	trace_qrtr_ns_server_add(srv->service, srv->instance,
255				 srv->node, srv->port);
256
257	return srv;
258
259err:
260	kfree(srv);
261	return NULL;
262}
263
264static 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, port);
271	if (!srv)
272		return -ENOENT;
273
274	xa_erase(&node->servers, port);
275
276	/* Broadcast the removal of local servers */
277	if (srv->node == qrtr_ns.local_node && bcast)
278		service_announce_del(&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(&lookup->sq, srv, false);
289	}
290
291	kfree(srv);
292
293	return 0;
294}
295
296static 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(qrtr_ns.sock, &msg, &iv, 1, 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 */
320static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
321{
322	int ret;
323
324	ret = say_hello(sq);
325	if (ret < 0)
326		return ret;
327
328	return announce_servers(sq);
329}
330
331static 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(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, srv->port, true);
353
354	/* Advertise the removal of this client to all local servers */
355	local_node = node_get(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(qrtr_ns.sock, &msg, &iv, 1, 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
380static 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(&lookup->li);
416		kfree(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, false);
426
427	/* Advertise the removal of this client to all local servers */
428	local_node = node_get(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(qrtr_ns.sock, &msg, &iv, 1, 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
454static 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(&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(&lookup->sq, srv, true);
490	}
491
492	return ret;
493}
494
495static 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, true);
516
517	return 0;
518}
519
520static 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(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(&lookup->li, &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, &filter))
550				continue;
551
552			lookup_notify(from, srv, true);
553		}
554	}
555
556	/* Empty notification, to indicate end of listing */
557	lookup_notify(from, NULL, true);
558
559	return 0;
560}
561
562static 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(&lookup->li);
581		kfree(lookup);
582	}
583}
584
585static 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(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(qrtr_ns.sock, &msg, &iv, 1,
609					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(qrtr_ctrl_pkt_strings[cmd],
624					      sq.sq_node, sq.sq_port);
625
626		ret = 0;
627		switch (cmd) {
628		case QRTR_TYPE_HELLO:
629			ret = ctrl_cmd_hello(&sq);
630			break;
631		case QRTR_TYPE_BYE:
632			ret = ctrl_cmd_bye(&sq);
633			break;
634		case QRTR_TYPE_DEL_CLIENT:
635			ret = ctrl_cmd_del_client(&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(&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(&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(&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(&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(recv_buf);
675}
676
677static void qrtr_ns_data_ready(struct sock *sk)
678{
679	trace_sk_data_ready(sk);
680
681	queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
682}
683
684int qrtr_ns_init(void)
685{
686	struct sockaddr_qrtr sq;
687	int ret;
688
689	INIT_LIST_HEAD(&qrtr_ns.lookups);
690	INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
691
692	ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
693			       PF_QIPCRTR, &qrtr_ns.sock);
694	if (ret < 0)
695		return ret;
696
697	ret = kernel_getsockname(qrtr_ns.sock, (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(qrtr_ns.sock, (struct sockaddr *)&sq, 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(&qrtr_ns.bcast_sq);
725	if (ret < 0)
726		goto err_wq;
727
728	return 0;
729
730err_wq:
731	destroy_workqueue(qrtr_ns.workqueue);
732err_sock:
733	sock_release(qrtr_ns.sock);
734	return ret;
735}
736EXPORT_SYMBOL_GPL(qrtr_ns_init);
737
738void qrtr_ns_remove(void)
739{
740	cancel_work_sync(&qrtr_ns.work);
741	destroy_workqueue(qrtr_ns.workqueue);
742	sock_release(qrtr_ns.sock);
743}
744EXPORT_SYMBOL_GPL(qrtr_ns_remove);
745
746MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
747MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
748MODULE_LICENSE("Dual BSD/GPL");
749