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 | ||
40e0b090 | 15 | #include <trace/events/sock.h> |
dfddb540 MS |
16 | #define CREATE_TRACE_POINTS |
17 | #include <trace/events/qrtr.h> | |
18 | ||
f26b32ef | 19 | static DEFINE_XARRAY(nodes); |
0c2204a4 MS |
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; | |
608a147a | 69 | struct xarray servers; |
0c2204a4 MS |
70 | }; |
71 | ||
72 | static struct qrtr_node *node_get(unsigned int node_id) | |
73 | { | |
74 | struct qrtr_node *node; | |
75 | ||
f26b32ef | 76 | node = xa_load(&nodes, node_id); |
0c2204a4 MS |
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) | |
9baeea50 | 83 | return NULL; |
0c2204a4 MS |
84 | |
85 | node->id = node_id; | |
608a147a | 86 | xa_init(&node->servers); |
0c2204a4 | 87 | |
f26b32ef | 88 | if (xa_store(&nodes, node_id, node, GFP_KERNEL)) { |
29de68c2 NP |
89 | kfree(node); |
90 | return NULL; | |
91 | } | |
0c2204a4 MS |
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 | ||
dfddb540 MS |
116 | trace_qrtr_ns_service_announce_new(srv->service, srv->instance, |
117 | srv->node, srv->port); | |
0c2204a4 MS |
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 | ||
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 | ||
dfddb540 MS |
143 | trace_qrtr_ns_service_announce_del(srv->service, srv->instance, |
144 | srv->node, srv->port); | |
0c2204a4 MS |
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) | |
13ef6ae8 | 161 | pr_err("failed to announce del service\n"); |
0c2204a4 MS |
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(qrtr_ns.sock, &msg, &iv, 1, 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 | { | |
0c2204a4 MS |
197 | struct qrtr_server *srv; |
198 | struct qrtr_node *node; | |
608a147a | 199 | unsigned long index; |
082bb94f | 200 | int ret; |
0c2204a4 MS |
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 */ | |
608a147a | 207 | xa_for_each(&node->servers, index, srv) { |
0c2204a4 MS |
208 | ret = service_announce_new(sq, srv); |
209 | if (ret < 0) { | |
210 | pr_err("failed to announce new service\n"); | |
082bb94f | 211 | return ret; |
0c2204a4 MS |
212 | } |
213 | } | |
082bb94f | 214 | return 0; |
0c2204a4 MS |
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(sizeof(*srv), GFP_KERNEL); | |
230 | if (!srv) | |
9baeea50 | 231 | return NULL; |
0c2204a4 MS |
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 */ | |
608a147a | 243 | old = xa_store(&node->servers, port, srv, GFP_KERNEL); |
0c2204a4 | 244 | if (old) { |
608a147a VV |
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 | } | |
0c2204a4 MS |
252 | } |
253 | ||
dfddb540 MS |
254 | trace_qrtr_ns_server_add(srv->service, srv->instance, |
255 | srv->node, srv->port); | |
0c2204a4 MS |
256 | |
257 | return srv; | |
258 | ||
259 | err: | |
260 | kfree(srv); | |
261 | return NULL; | |
262 | } | |
263 | ||
839349d1 | 264 | static int server_del(struct qrtr_node *node, unsigned int port, bool bcast) |
0c2204a4 MS |
265 | { |
266 | struct qrtr_lookup *lookup; | |
267 | struct qrtr_server *srv; | |
268 | struct list_head *li; | |
269 | ||
608a147a | 270 | srv = xa_load(&node->servers, port); |
0c2204a4 MS |
271 | if (!srv) |
272 | return -ENOENT; | |
273 | ||
608a147a | 274 | xa_erase(&node->servers, port); |
0c2204a4 MS |
275 | |
276 | /* Broadcast the removal of local servers */ | |
839349d1 | 277 | if (srv->node == qrtr_ns.local_node && bcast) |
0c2204a4 MS |
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 | ||
a1dc1d6a BA |
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(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 | ||
0c2204a4 MS |
319 | /* Announce the list of servers registered on the local node */ |
320 | static int ctrl_cmd_hello(struct sockaddr_qrtr *sq) | |
321 | { | |
a1dc1d6a BA |
322 | int ret; |
323 | ||
324 | ret = say_hello(sq); | |
325 | if (ret < 0) | |
326 | return ret; | |
327 | ||
0c2204a4 MS |
328 | return announce_servers(sq); |
329 | } | |
330 | ||
331 | static int ctrl_cmd_bye(struct sockaddr_qrtr *from) | |
332 | { | |
333 | struct qrtr_node *local_node; | |
0c2204a4 MS |
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; | |
608a147a | 339 | unsigned long index; |
0c2204a4 | 340 | struct kvec iv; |
082bb94f | 341 | int ret; |
0c2204a4 MS |
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 */ | |
608a147a | 351 | xa_for_each(&node->servers, index, srv) |
839349d1 | 352 | server_del(node, srv->port, true); |
0c2204a4 MS |
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 | ||
608a147a | 363 | xa_for_each(&local_node->servers, index, srv) { |
0c2204a4 MS |
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"); | |
082bb94f | 374 | return ret; |
0c2204a4 MS |
375 | } |
376 | } | |
082bb94f | 377 | return 0; |
0c2204a4 MS |
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; | |
0c2204a4 MS |
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; | |
608a147a | 392 | unsigned long index; |
0c2204a4 | 393 | struct kvec iv; |
082bb94f | 394 | int ret; |
0c2204a4 MS |
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 | ||
839349d1 SR |
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 | */ | |
0c2204a4 MS |
423 | node = node_get(node_id); |
424 | if (node) | |
839349d1 | 425 | server_del(node, port, false); |
0c2204a4 MS |
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 | ||
608a147a | 437 | xa_for_each(&local_node->servers, index, srv) { |
0c2204a4 MS |
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"); | |
082bb94f | 448 | return ret; |
0c2204a4 MS |
449 | } |
450 | } | |
082bb94f | 451 | return 0; |
0c2204a4 MS |
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 | ||
0c2204a4 MS |
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 | ||
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 | ||
0c2204a4 MS |
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 | ||
9bf2e916 S |
515 | server_del(node, port, true); |
516 | ||
517 | return 0; | |
0c2204a4 MS |
518 | } |
519 | ||
520 | static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from, | |
521 | unsigned int service, unsigned int instance) | |
522 | { | |
0c2204a4 | 523 | struct qrtr_server_filter filter; |
0c2204a4 | 524 | struct qrtr_lookup *lookup; |
608a147a | 525 | struct qrtr_server *srv; |
0c2204a4 | 526 | struct qrtr_node *node; |
608a147a VV |
527 | unsigned long node_idx; |
528 | unsigned long srv_idx; | |
0c2204a4 MS |
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 | ||
608a147a VV |
547 | xa_for_each(&nodes, node_idx, node) { |
548 | xa_for_each(&node->servers, srv_idx, srv) { | |
0c2204a4 MS |
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 | ||
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(&lookup->li); | |
581 | kfree(lookup); | |
582 | } | |
583 | } | |
584 | ||
0c2204a4 MS |
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(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]) | |
dfddb540 MS |
623 | trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd], |
624 | sq.sq_node, sq.sq_port); | |
0c2204a4 MS |
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 | ||
677 | static void qrtr_ns_data_ready(struct sock *sk) | |
678 | { | |
40e0b090 PY |
679 | trace_sk_data_ready(sk); |
680 | ||
0c2204a4 MS |
681 | queue_work(qrtr_ns.workqueue, &qrtr_ns.work); |
682 | } | |
683 | ||
4beb17e5 | 684 | int qrtr_ns_init(void) |
0c2204a4 MS |
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) | |
4beb17e5 | 695 | return ret; |
0c2204a4 MS |
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 | ||
022acfa6 | 703 | qrtr_ns.workqueue = alloc_ordered_workqueue("qrtr_ns_handler", 0); |
a49e72b3 WY |
704 | if (!qrtr_ns.workqueue) { |
705 | ret = -ENOMEM; | |
c6e08d62 | 706 | goto err_sock; |
a49e72b3 | 707 | } |
c6e08d62 | 708 | |
0c2204a4 MS |
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"); | |
c6e08d62 | 717 | goto err_wq; |
0c2204a4 MS |
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 | ||
a1dc1d6a | 724 | ret = say_hello(&qrtr_ns.bcast_sq); |
0c2204a4 MS |
725 | if (ret < 0) |
726 | goto err_wq; | |
727 | ||
4beb17e5 | 728 | return 0; |
0c2204a4 MS |
729 | |
730 | err_wq: | |
731 | destroy_workqueue(qrtr_ns.workqueue); | |
732 | err_sock: | |
733 | sock_release(qrtr_ns.sock); | |
4beb17e5 | 734 | return ret; |
0c2204a4 MS |
735 | } |
736 | EXPORT_SYMBOL_GPL(qrtr_ns_init); | |
737 | ||
738 | void 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 | } | |
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"); |