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 18 static RADIX_TREE(nodes, GFP_KERNEL); 19 20 static 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 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", 40 }; 41 42 struct qrtr_server_filter { 43 unsigned int service; 44 unsigned int instance; 45 unsigned int ifilter; 46 }; 47 48 struct qrtr_lookup { 49 unsigned int service; 50 unsigned int instance; 51 52 struct sockaddr_qrtr sq; 53 struct list_head li; 54 }; 55 56 struct 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 66 struct qrtr_node { 67 unsigned int id; 68 struct radix_tree_root servers; 69 }; 70 71 static 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 94 static 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 107 static 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 133 static 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 164 static 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 193 static 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 232 static 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 271 err: 272 kfree(srv); 273 return NULL; 274 } 275 276 static int server_del(struct qrtr_node *node, unsigned int port) 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) 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 308 static 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 */ 332 static 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 343 static 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); 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 421 static 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 */ 462 node = node_get(node_id); 463 if (node) 464 server_del(node, port); 465 466 /* Advertise the removal of this client to all local servers */ 467 local_node = node_get(qrtr_ns.local_node); 468 if (!local_node) 469 return 0; 470 471 memset(&pkt, 0, sizeof(pkt)); 472 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT); 473 pkt.client.node = cpu_to_le32(node_id); 474 pkt.client.port = cpu_to_le32(port); 475 476 rcu_read_lock(); 477 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { 478 srv = radix_tree_deref_slot(slot); 479 if (!srv) 480 continue; 481 if (radix_tree_deref_retry(srv)) { 482 slot = radix_tree_iter_retry(&iter); 483 continue; 484 } 485 slot = radix_tree_iter_resume(slot, &iter); 486 rcu_read_unlock(); 487 488 sq.sq_family = AF_QIPCRTR; 489 sq.sq_node = srv->node; 490 sq.sq_port = srv->port; 491 492 msg.msg_name = (struct sockaddr *)&sq; 493 msg.msg_namelen = sizeof(sq); 494 495 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); 496 if (ret < 0) { 497 pr_err("failed to send del client cmd\n"); 498 return ret; 499 } 500 rcu_read_lock(); 501 } 502 503 rcu_read_unlock(); 504 505 return 0; 506 } 507 508 static int ctrl_cmd_new_server(struct sockaddr_qrtr *from, 509 unsigned int service, unsigned int instance, 510 unsigned int node_id, unsigned int port) 511 { 512 struct qrtr_lookup *lookup; 513 struct qrtr_server *srv; 514 struct list_head *li; 515 int ret = 0; 516 517 /* Ignore specified node and port for local servers */ 518 if (from->sq_node == qrtr_ns.local_node) { 519 node_id = from->sq_node; 520 port = from->sq_port; 521 } 522 523 srv = server_add(service, instance, node_id, port); 524 if (!srv) 525 return -EINVAL; 526 527 if (srv->node == qrtr_ns.local_node) { 528 ret = service_announce_new(&qrtr_ns.bcast_sq, srv); 529 if (ret < 0) { 530 pr_err("failed to announce new service\n"); 531 return ret; 532 } 533 } 534 535 /* Notify any potential lookups about the new server */ 536 list_for_each(li, &qrtr_ns.lookups) { 537 lookup = container_of(li, struct qrtr_lookup, li); 538 if (lookup->service && lookup->service != service) 539 continue; 540 if (lookup->instance && lookup->instance != instance) 541 continue; 542 543 lookup_notify(&lookup->sq, srv, true); 544 } 545 546 return ret; 547 } 548 549 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from, 550 unsigned int service, unsigned int instance, 551 unsigned int node_id, unsigned int port) 552 { 553 struct qrtr_node *node; 554 555 /* Ignore specified node and port for local servers*/ 556 if (from->sq_node == qrtr_ns.local_node) { 557 node_id = from->sq_node; 558 port = from->sq_port; 559 } 560 561 /* Local servers may only unregister themselves */ 562 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) 563 return -EINVAL; 564 565 node = node_get(node_id); 566 if (!node) 567 return -ENOENT; 568 569 return server_del(node, port); 570 } 571 572 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from, 573 unsigned int service, unsigned int instance) 574 { 575 struct radix_tree_iter node_iter; 576 struct qrtr_server_filter filter; 577 struct radix_tree_iter srv_iter; 578 struct qrtr_lookup *lookup; 579 struct qrtr_node *node; 580 void __rcu **node_slot; 581 void __rcu **srv_slot; 582 583 /* Accept only local observers */ 584 if (from->sq_node != qrtr_ns.local_node) 585 return -EINVAL; 586 587 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL); 588 if (!lookup) 589 return -ENOMEM; 590 591 lookup->sq = *from; 592 lookup->service = service; 593 lookup->instance = instance; 594 list_add_tail(&lookup->li, &qrtr_ns.lookups); 595 596 memset(&filter, 0, sizeof(filter)); 597 filter.service = service; 598 filter.instance = instance; 599 600 rcu_read_lock(); 601 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) { 602 node = radix_tree_deref_slot(node_slot); 603 if (!node) 604 continue; 605 if (radix_tree_deref_retry(node)) { 606 node_slot = radix_tree_iter_retry(&node_iter); 607 continue; 608 } 609 node_slot = radix_tree_iter_resume(node_slot, &node_iter); 610 611 radix_tree_for_each_slot(srv_slot, &node->servers, 612 &srv_iter, 0) { 613 struct qrtr_server *srv; 614 615 srv = radix_tree_deref_slot(srv_slot); 616 if (!srv) 617 continue; 618 if (radix_tree_deref_retry(srv)) { 619 srv_slot = radix_tree_iter_retry(&srv_iter); 620 continue; 621 } 622 623 if (!server_match(srv, &filter)) 624 continue; 625 626 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter); 627 628 rcu_read_unlock(); 629 lookup_notify(from, srv, true); 630 rcu_read_lock(); 631 } 632 } 633 rcu_read_unlock(); 634 635 /* Empty notification, to indicate end of listing */ 636 lookup_notify(from, NULL, true); 637 638 return 0; 639 } 640 641 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from, 642 unsigned int service, unsigned int instance) 643 { 644 struct qrtr_lookup *lookup; 645 struct list_head *tmp; 646 struct list_head *li; 647 648 list_for_each_safe(li, tmp, &qrtr_ns.lookups) { 649 lookup = container_of(li, struct qrtr_lookup, li); 650 if (lookup->sq.sq_node != from->sq_node) 651 continue; 652 if (lookup->sq.sq_port != from->sq_port) 653 continue; 654 if (lookup->service != service) 655 continue; 656 if (lookup->instance && lookup->instance != instance) 657 continue; 658 659 list_del(&lookup->li); 660 kfree(lookup); 661 } 662 } 663 664 static void qrtr_ns_worker(struct work_struct *work) 665 { 666 const struct qrtr_ctrl_pkt *pkt; 667 size_t recv_buf_size = 4096; 668 struct sockaddr_qrtr sq; 669 struct msghdr msg = { }; 670 unsigned int cmd; 671 ssize_t msglen; 672 void *recv_buf; 673 struct kvec iv; 674 int ret; 675 676 msg.msg_name = (struct sockaddr *)&sq; 677 msg.msg_namelen = sizeof(sq); 678 679 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL); 680 if (!recv_buf) 681 return; 682 683 for (;;) { 684 iv.iov_base = recv_buf; 685 iv.iov_len = recv_buf_size; 686 687 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1, 688 iv.iov_len, MSG_DONTWAIT); 689 690 if (msglen == -EAGAIN) 691 break; 692 693 if (msglen < 0) { 694 pr_err("error receiving packet: %zd\n", msglen); 695 break; 696 } 697 698 pkt = recv_buf; 699 cmd = le32_to_cpu(pkt->cmd); 700 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) && 701 qrtr_ctrl_pkt_strings[cmd]) 702 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd], 703 sq.sq_node, sq.sq_port); 704 705 ret = 0; 706 switch (cmd) { 707 case QRTR_TYPE_HELLO: 708 ret = ctrl_cmd_hello(&sq); 709 break; 710 case QRTR_TYPE_BYE: 711 ret = ctrl_cmd_bye(&sq); 712 break; 713 case QRTR_TYPE_DEL_CLIENT: 714 ret = ctrl_cmd_del_client(&sq, 715 le32_to_cpu(pkt->client.node), 716 le32_to_cpu(pkt->client.port)); 717 break; 718 case QRTR_TYPE_NEW_SERVER: 719 ret = ctrl_cmd_new_server(&sq, 720 le32_to_cpu(pkt->server.service), 721 le32_to_cpu(pkt->server.instance), 722 le32_to_cpu(pkt->server.node), 723 le32_to_cpu(pkt->server.port)); 724 break; 725 case QRTR_TYPE_DEL_SERVER: 726 ret = ctrl_cmd_del_server(&sq, 727 le32_to_cpu(pkt->server.service), 728 le32_to_cpu(pkt->server.instance), 729 le32_to_cpu(pkt->server.node), 730 le32_to_cpu(pkt->server.port)); 731 break; 732 case QRTR_TYPE_EXIT: 733 case QRTR_TYPE_PING: 734 case QRTR_TYPE_RESUME_TX: 735 break; 736 case QRTR_TYPE_NEW_LOOKUP: 737 ret = ctrl_cmd_new_lookup(&sq, 738 le32_to_cpu(pkt->server.service), 739 le32_to_cpu(pkt->server.instance)); 740 break; 741 case QRTR_TYPE_DEL_LOOKUP: 742 ctrl_cmd_del_lookup(&sq, 743 le32_to_cpu(pkt->server.service), 744 le32_to_cpu(pkt->server.instance)); 745 break; 746 } 747 748 if (ret < 0) 749 pr_err("failed while handling packet from %d:%d", 750 sq.sq_node, sq.sq_port); 751 } 752 753 kfree(recv_buf); 754 } 755 756 static void qrtr_ns_data_ready(struct sock *sk) 757 { 758 queue_work(qrtr_ns.workqueue, &qrtr_ns.work); 759 } 760 761 int qrtr_ns_init(void) 762 { 763 struct sockaddr_qrtr sq; 764 int ret; 765 766 INIT_LIST_HEAD(&qrtr_ns.lookups); 767 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker); 768 769 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM, 770 PF_QIPCRTR, &qrtr_ns.sock); 771 if (ret < 0) 772 return ret; 773 774 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq); 775 if (ret < 0) { 776 pr_err("failed to get socket name\n"); 777 goto err_sock; 778 } 779 780 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1); 781 if (!qrtr_ns.workqueue) { 782 ret = -ENOMEM; 783 goto err_sock; 784 } 785 786 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready; 787 788 sq.sq_port = QRTR_PORT_CTRL; 789 qrtr_ns.local_node = sq.sq_node; 790 791 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq)); 792 if (ret < 0) { 793 pr_err("failed to bind to socket\n"); 794 goto err_wq; 795 } 796 797 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR; 798 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST; 799 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL; 800 801 ret = say_hello(&qrtr_ns.bcast_sq); 802 if (ret < 0) 803 goto err_wq; 804 805 return 0; 806 807 err_wq: 808 destroy_workqueue(qrtr_ns.workqueue); 809 err_sock: 810 sock_release(qrtr_ns.sock); 811 return ret; 812 } 813 EXPORT_SYMBOL_GPL(qrtr_ns_init); 814 815 void qrtr_ns_remove(void) 816 { 817 cancel_work_sync(&qrtr_ns.work); 818 destroy_workqueue(qrtr_ns.workqueue); 819 sock_release(qrtr_ns.sock); 820 } 821 EXPORT_SYMBOL_GPL(qrtr_ns_remove); 822 823 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>"); 824 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice"); 825 MODULE_LICENSE("Dual BSD/GPL"); 826