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#define CREATE_TRACE_POINTS 16#include <trace/events/qrtr.h> 17 18static RADIX_TREE(nodes, GFP_KERNEL); 19 20static struct { 21 struct socket *sock; 22 struct sockaddr_qrtr bcast_sq; 23 struct list_head lookups; 24 struct workqueue_struct *workqueue; 25 struct work_struct work; 26 int local_node; 27} qrtr_ns; 28 29static 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", 40}; 41 42struct qrtr_server_filter { 43 unsigned int service; 44 unsigned int instance; 45 unsigned int ifilter; 46}; 47 48struct qrtr_lookup { 49 unsigned int service; 50 unsigned int instance; 51 52 struct sockaddr_qrtr sq; 53 struct list_head li; 54}; 55 56struct qrtr_server { 57 unsigned int service; 58 unsigned int instance; 59 60 unsigned int node; 61 unsigned int port; 62 63 struct list_head qli; 64}; 65 66struct qrtr_node { 67 unsigned int id; 68 struct radix_tree_root servers; 69}; 70 71static struct qrtr_node *node_get(unsigned int node_id) 72{ 73 struct qrtr_node *node; 74 75 node = radix_tree_lookup(&nodes, node_id); 76 if (node) 77 return node; 78 79 /* If node didn't exist, allocate and insert it to the tree */ 80 node = kzalloc(sizeof(*node), GFP_KERNEL); 81 if (!node) 82 return NULL; 83 84 node->id = node_id; 85 86 if (radix_tree_insert(&nodes, node_id, node)) { 87 kfree(node); 88 return NULL; 89 } 90 91 return node; 92} 93 94static int server_match(const struct qrtr_server *srv, 95 const struct qrtr_server_filter *f) 96{ 97 unsigned int ifilter = f->ifilter; 98 99 if (f->service != 0 && srv->service != f->service) 100 return 0; 101 if (!ifilter && f->instance) 102 ifilter = ~0; 103 104 return (srv->instance & ifilter) == f->instance; 105} 106 107static int service_announce_new(struct sockaddr_qrtr *dest, 108 struct qrtr_server *srv) 109{ 110 struct qrtr_ctrl_pkt pkt; 111 struct msghdr msg = { }; 112 struct kvec iv; 113 114 trace_qrtr_ns_service_announce_new(srv->service, srv->instance, 115 srv->node, srv->port); 116 117 iv.iov_base = &pkt; 118 iv.iov_len = sizeof(pkt); 119 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); 126 127 msg.msg_name = (struct sockaddr *)dest; 128 msg.msg_namelen = sizeof(*dest); 129 130 return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 131} 132 133static int service_announce_del(struct sockaddr_qrtr *dest, 134 struct qrtr_server *srv) 135{ 136 struct qrtr_ctrl_pkt pkt; 137 struct msghdr msg = { }; 138 struct kvec iv; 139 int ret; 140 141 trace_qrtr_ns_service_announce_del(srv->service, srv->instance, 142 srv->node, srv->port); 143 144 iv.iov_base = &pkt; 145 iv.iov_len = sizeof(pkt); 146 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); 153 154 msg.msg_name = (struct sockaddr *)dest; 155 msg.msg_namelen = sizeof(*dest); 156 157 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 158 if (ret < 0) 159 pr_err("failed to announce del service\n"); 160 161 return ret; 162} 163 164static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv, 165 bool new) 166{ 167 struct qrtr_ctrl_pkt pkt; 168 struct msghdr msg = { }; 169 struct kvec iv; 170 int ret; 171 172 iv.iov_base = &pkt; 173 iv.iov_len = sizeof(pkt); 174 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); 178 if (srv) { 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); 183 } 184 185 msg.msg_name = (struct sockaddr *)to; 186 msg.msg_namelen = sizeof(*to); 187 188 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 189 if (ret < 0) 190 pr_err("failed to send lookup notification\n"); 191} 192 193static int announce_servers(struct sockaddr_qrtr *sq) 194{ 195 struct radix_tree_iter iter; 196 struct qrtr_server *srv; 197 struct qrtr_node *node; 198 void __rcu **slot; 199 int ret; 200 201 node = node_get(qrtr_ns.local_node); 202 if (!node) 203 return 0; 204 205 rcu_read_lock(); 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); 209 if (!srv) 210 continue; 211 if (radix_tree_deref_retry(srv)) { 212 slot = radix_tree_iter_retry(&iter); 213 continue; 214 } 215 slot = radix_tree_iter_resume(slot, &iter); 216 rcu_read_unlock(); 217 218 ret = service_announce_new(sq, srv); 219 if (ret < 0) { 220 pr_err("failed to announce new service\n"); 221 return ret; 222 } 223 224 rcu_read_lock(); 225 } 226 227 rcu_read_unlock(); 228 229 return 0; 230} 231 232static struct qrtr_server *server_add(unsigned int service, 233 unsigned int instance, 234 unsigned int node_id, 235 unsigned int port) 236{ 237 struct qrtr_server *srv; 238 struct qrtr_server *old; 239 struct qrtr_node *node; 240 241 if (!service || !port) 242 return NULL; 243 244 srv = kzalloc(sizeof(*srv), GFP_KERNEL); 245 if (!srv) 246 return NULL; 247 248 srv->service = service; 249 srv->instance = instance; 250 srv->node = node_id; 251 srv->port = port; 252 253 node = node_get(node_id); 254 if (!node) 255 goto err; 256 257 /* Delete the old server on the same port */ 258 old = radix_tree_lookup(&node->servers, port); 259 if (old) { 260 radix_tree_delete(&node->servers, port); 261 kfree(old); 262 } 263 264 radix_tree_insert(&node->servers, port, srv); 265 266 trace_qrtr_ns_server_add(srv->service, srv->instance, 267 srv->node, srv->port); 268 269 return srv; 270 271err: 272 kfree(srv); 273 return NULL; 274} 275 276static int server_del(struct qrtr_node *node, unsigned int port, bool bcast) 277{ 278 struct qrtr_lookup *lookup; 279 struct qrtr_server *srv; 280 struct list_head *li; 281 282 srv = radix_tree_lookup(&node->servers, port); 283 if (!srv) 284 return -ENOENT; 285 286 radix_tree_delete(&node->servers, port); 287 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); 291 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) 296 continue; 297 if (lookup->instance && lookup->instance != srv->instance) 298 continue; 299 300 lookup_notify(&lookup->sq, srv, false); 301 } 302 303 kfree(srv); 304 305 return 0; 306} 307 308static int say_hello(struct sockaddr_qrtr *dest) 309{ 310 struct qrtr_ctrl_pkt pkt; 311 struct msghdr msg = { }; 312 struct kvec iv; 313 int ret; 314 315 iv.iov_base = &pkt; 316 iv.iov_len = sizeof(pkt); 317 318 memset(&pkt, 0, sizeof(pkt)); 319 pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO); 320 321 msg.msg_name = (struct sockaddr *)dest; 322 msg.msg_namelen = sizeof(*dest); 323 324 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 325 if (ret < 0) 326 pr_err("failed to send hello msg\n"); 327 328 return ret; 329} 330 331/* Announce the list of servers registered on the local node */ 332static int ctrl_cmd_hello(struct sockaddr_qrtr *sq) 333{ 334 int ret; 335 336 ret = say_hello(sq); 337 if (ret < 0) 338 return ret; 339 340 return announce_servers(sq); 341} 342 343static int ctrl_cmd_bye(struct sockaddr_qrtr *from) 344{ 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; 352 void __rcu **slot; 353 struct kvec iv; 354 int ret; 355 356 iv.iov_base = &pkt; 357 iv.iov_len = sizeof(pkt); 358 359 node = node_get(from->sq_node); 360 if (!node) 361 return 0; 362 363 rcu_read_lock(); 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); 367 if (!srv) 368 continue; 369 if (radix_tree_deref_retry(srv)) { 370 slot = radix_tree_iter_retry(&iter); 371 continue; 372 } 373 slot = radix_tree_iter_resume(slot, &iter); 374 rcu_read_unlock(); 375 server_del(node, srv->port, true); 376 rcu_read_lock(); 377 } 378 rcu_read_unlock(); 379 380 /* Advertise the removal of this client to all local servers */ 381 local_node = node_get(qrtr_ns.local_node); 382 if (!local_node) 383 return 0; 384 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); 388 389 rcu_read_lock(); 390 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { 391 srv = radix_tree_deref_slot(slot); 392 if (!srv) 393 continue; 394 if (radix_tree_deref_retry(srv)) { 395 slot = radix_tree_iter_retry(&iter); 396 continue; 397 } 398 slot = radix_tree_iter_resume(slot, &iter); 399 rcu_read_unlock(); 400 401 sq.sq_family = AF_QIPCRTR; 402 sq.sq_node = srv->node; 403 sq.sq_port = srv->port; 404 405 msg.msg_name = (struct sockaddr *)&sq; 406 msg.msg_namelen = sizeof(sq); 407 408 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 409 if (ret < 0) { 410 pr_err("failed to send bye cmd\n"); 411 return ret; 412 } 413 rcu_read_lock(); 414 } 415 416 rcu_read_unlock(); 417 418 return 0; 419} 420 421static int ctrl_cmd_del_client(struct sockaddr_qrtr *from, 422 unsigned int node_id, unsigned int port) 423{ 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; 434 void __rcu **slot; 435 struct kvec iv; 436 int ret; 437 438 iv.iov_base = &pkt; 439 iv.iov_len = sizeof(pkt); 440 441 /* Don't accept spoofed messages */ 442 if (from->sq_node != node_id) 443 return -EINVAL; 444 445 /* Local DEL_CLIENT messages comes from the port being closed */ 446 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) 447 return -EINVAL; 448 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) 453 continue; 454 if (lookup->sq.sq_port != port) 455 continue; 456 457 list_del(&lookup->li); 458 kfree(lookup); 459 } 460 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(). 464 */ 465 node = node_get(node_id); 466 if (node) 467 server_del(node, port, false); 468 469 /* Advertise the removal of this client to all local servers */ 470 local_node = node_get(qrtr_ns.local_node); 471 if (!local_node) 472 return 0; 473 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); 478 479 rcu_read_lock(); 480 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { 481 srv = radix_tree_deref_slot(slot); 482 if (!srv) 483 continue; 484 if (radix_tree_deref_retry(srv)) { 485 slot = radix_tree_iter_retry(&iter); 486 continue; 487 } 488 slot = radix_tree_iter_resume(slot, &iter); 489 rcu_read_unlock(); 490 491 sq.sq_family = AF_QIPCRTR; 492 sq.sq_node = srv->node; 493 sq.sq_port = srv->port; 494 495 msg.msg_name = (struct sockaddr *)&sq; 496 msg.msg_namelen = sizeof(sq); 497 498 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 499 if (ret < 0) { 500 pr_err("failed to send del client cmd\n"); 501 return ret; 502 } 503 rcu_read_lock(); 504 } 505 506 rcu_read_unlock(); 507 508 return 0; 509} 510 511static int ctrl_cmd_new_server(struct sockaddr_qrtr *from, 512 unsigned int service, unsigned int instance, 513 unsigned int node_id, unsigned int port) 514{ 515 struct qrtr_lookup *lookup; 516 struct qrtr_server *srv; 517 struct list_head *li; 518 int ret = 0; 519 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; 524 } 525 526 /* Don't accept spoofed messages */ 527 if (from->sq_node != node_id) 528 return -EINVAL; 529 530 srv = server_add(service, instance, node_id, port); 531 if (!srv) 532 return -EINVAL; 533 534 if (srv->node == qrtr_ns.local_node) { 535 ret = service_announce_new(&qrtr_ns.bcast_sq, srv); 536 if (ret < 0) { 537 pr_err("failed to announce new service\n"); 538 return ret; 539 } 540 } 541 542 /* Notify any potential lookups about the new server */ 543 list_for_each(li, &qrtr_ns.lookups) { 544 lookup = container_of(li, struct qrtr_lookup, li); 545 if (lookup->service && lookup->service != service) 546 continue; 547 if (lookup->instance && lookup->instance != instance) 548 continue; 549 550 lookup_notify(&lookup->sq, srv, true); 551 } 552 553 return ret; 554} 555 556static int ctrl_cmd_del_server(struct sockaddr_qrtr *from, 557 unsigned int service, unsigned int instance, 558 unsigned int node_id, unsigned int port) 559{ 560 struct qrtr_node *node; 561 562 /* Ignore specified node and port for local servers*/ 563 if (from->sq_node == qrtr_ns.local_node) { 564 node_id = from->sq_node; 565 port = from->sq_port; 566 } 567 568 /* Don't accept spoofed messages */ 569 if (from->sq_node != node_id) 570 return -EINVAL; 571 572 /* Local servers may only unregister themselves */ 573 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) 574 return -EINVAL; 575 576 node = node_get(node_id); 577 if (!node) 578 return -ENOENT; 579 580 server_del(node, port, true); 581 582 return 0; 583} 584 585static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from, 586 unsigned int service, unsigned int instance) 587{ 588 struct radix_tree_iter node_iter; 589 struct qrtr_server_filter filter; 590 struct radix_tree_iter srv_iter; 591 struct qrtr_lookup *lookup; 592 struct qrtr_node *node; 593 void __rcu **node_slot; 594 void __rcu **srv_slot; 595 596 /* Accept only local observers */ 597 if (from->sq_node != qrtr_ns.local_node) 598 return -EINVAL; 599 600 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL); 601 if (!lookup) 602 return -ENOMEM; 603 604 lookup->sq = *from; 605 lookup->service = service; 606 lookup->instance = instance; 607 list_add_tail(&lookup->li, &qrtr_ns.lookups); 608 609 memset(&filter, 0, sizeof(filter)); 610 filter.service = service; 611 filter.instance = instance; 612 613 rcu_read_lock(); 614 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) { 615 node = radix_tree_deref_slot(node_slot); 616 if (!node) 617 continue; 618 if (radix_tree_deref_retry(node)) { 619 node_slot = radix_tree_iter_retry(&node_iter); 620 continue; 621 } 622 node_slot = radix_tree_iter_resume(node_slot, &node_iter); 623 624 radix_tree_for_each_slot(srv_slot, &node->servers, 625 &srv_iter, 0) { 626 struct qrtr_server *srv; 627 628 srv = radix_tree_deref_slot(srv_slot); 629 if (!srv) 630 continue; 631 if (radix_tree_deref_retry(srv)) { 632 srv_slot = radix_tree_iter_retry(&srv_iter); 633 continue; 634 } 635 636 if (!server_match(srv, &filter)) 637 continue; 638 639 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter); 640 641 rcu_read_unlock(); 642 lookup_notify(from, srv, true); 643 rcu_read_lock(); 644 } 645 } 646 rcu_read_unlock(); 647 648 /* Empty notification, to indicate end of listing */ 649 lookup_notify(from, NULL, true); 650 651 return 0; 652} 653 654static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from, 655 unsigned int service, unsigned int instance) 656{ 657 struct qrtr_lookup *lookup; 658 struct list_head *tmp; 659 struct list_head *li; 660 661 list_for_each_safe(li, tmp, &qrtr_ns.lookups) { 662 lookup = container_of(li, struct qrtr_lookup, li); 663 if (lookup->sq.sq_node != from->sq_node) 664 continue; 665 if (lookup->sq.sq_port != from->sq_port) 666 continue; 667 if (lookup->service != service) 668 continue; 669 if (lookup->instance && lookup->instance != instance) 670 continue; 671 672 list_del(&lookup->li); 673 kfree(lookup); 674 } 675} 676 677static void qrtr_ns_worker(struct work_struct *work) 678{ 679 const struct qrtr_ctrl_pkt *pkt; 680 size_t recv_buf_size = 4096; 681 struct sockaddr_qrtr sq; 682 struct msghdr msg = { }; 683 unsigned int cmd; 684 ssize_t msglen; 685 void *recv_buf; 686 struct kvec iv; 687 int ret; 688 689 msg.msg_name = (struct sockaddr *)&sq; 690 msg.msg_namelen = sizeof(sq); 691 692 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL); 693 if (!recv_buf) 694 return; 695 696 for (;;) { 697 iv.iov_base = recv_buf; 698 iv.iov_len = recv_buf_size; 699 700 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1, 701 iv.iov_len, MSG_DONTWAIT); 702 703 if (msglen == -EAGAIN) 704 break; 705 706 if (msglen < 0) { 707 pr_err("error receiving packet: %zd\n", msglen); 708 break; 709 } 710 711 pkt = recv_buf; 712 cmd = le32_to_cpu(pkt->cmd); 713 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) && 714 qrtr_ctrl_pkt_strings[cmd]) 715 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd], 716 sq.sq_node, sq.sq_port); 717 718 ret = 0; 719 switch (cmd) { 720 case QRTR_TYPE_HELLO: 721 ret = ctrl_cmd_hello(&sq); 722 break; 723 case QRTR_TYPE_BYE: 724 ret = ctrl_cmd_bye(&sq); 725 break; 726 case QRTR_TYPE_DEL_CLIENT: 727 ret = ctrl_cmd_del_client(&sq, 728 le32_to_cpu(pkt->client.node), 729 le32_to_cpu(pkt->client.port)); 730 break; 731 case QRTR_TYPE_NEW_SERVER: 732 ret = ctrl_cmd_new_server(&sq, 733 le32_to_cpu(pkt->server.service), 734 le32_to_cpu(pkt->server.instance), 735 le32_to_cpu(pkt->server.node), 736 le32_to_cpu(pkt->server.port)); 737 break; 738 case QRTR_TYPE_DEL_SERVER: 739 ret = ctrl_cmd_del_server(&sq, 740 le32_to_cpu(pkt->server.service), 741 le32_to_cpu(pkt->server.instance), 742 le32_to_cpu(pkt->server.node), 743 le32_to_cpu(pkt->server.port)); 744 break; 745 case QRTR_TYPE_EXIT: 746 case QRTR_TYPE_PING: 747 case QRTR_TYPE_RESUME_TX: 748 break; 749 case QRTR_TYPE_NEW_LOOKUP: 750 ret = ctrl_cmd_new_lookup(&sq, 751 le32_to_cpu(pkt->server.service), 752 le32_to_cpu(pkt->server.instance)); 753 break; 754 case QRTR_TYPE_DEL_LOOKUP: 755 ctrl_cmd_del_lookup(&sq, 756 le32_to_cpu(pkt->server.service), 757 le32_to_cpu(pkt->server.instance)); 758 break; 759 } 760 761 if (ret < 0) 762 pr_err("failed while handling packet from %d:%d", 763 sq.sq_node, sq.sq_port); 764 } 765 766 kfree(recv_buf); 767} 768 769static void qrtr_ns_data_ready(struct sock *sk) 770{ 771 queue_work(qrtr_ns.workqueue, &qrtr_ns.work); 772} 773 774void qrtr_ns_init(void) 775{ 776 struct sockaddr_qrtr sq; 777 int ret; 778 779 INIT_LIST_HEAD(&qrtr_ns.lookups); 780 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker); 781 782 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM, 783 PF_QIPCRTR, &qrtr_ns.sock); 784 if (ret < 0) 785 return; 786 787 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq); 788 if (ret < 0) { 789 pr_err("failed to get socket name\n"); 790 goto err_sock; 791 } 792 793 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1); 794 if (!qrtr_ns.workqueue) { 795 ret = -ENOMEM; 796 goto err_sock; 797 } 798 799 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready; 800 801 sq.sq_port = QRTR_PORT_CTRL; 802 qrtr_ns.local_node = sq.sq_node; 803 804 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq)); 805 if (ret < 0) { 806 pr_err("failed to bind to socket\n"); 807 goto err_wq; 808 } 809 810 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR; 811 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST; 812 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL; 813 814 ret = say_hello(&qrtr_ns.bcast_sq); 815 if (ret < 0) 816 goto err_wq; 817 818 return; 819 820err_wq: 821 destroy_workqueue(qrtr_ns.workqueue); 822err_sock: 823 sock_release(qrtr_ns.sock); 824} 825EXPORT_SYMBOL_GPL(qrtr_ns_init); 826 827void qrtr_ns_remove(void) 828{ 829 cancel_work_sync(&qrtr_ns.work); 830 destroy_workqueue(qrtr_ns.workqueue); 831 sock_release(qrtr_ns.sock); 832} 833EXPORT_SYMBOL_GPL(qrtr_ns_remove); 834 835MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>"); 836MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice"); 837MODULE_LICENSE("Dual BSD/GPL"); 838